From a94096b64da6cf466235c1729f44b449d1541db9 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Thu, 10 May 2018 06:56:13 -0700 Subject: [PATCH] Make subcomponent errors to always set relevant axis error; use do_checks and set_error --- Firmware/.vscode/c_cpp_properties.json | 6 +++-- Firmware/MotorControl/axis.cpp | 34 +++++++++++++++++--------- Firmware/MotorControl/axis.hpp | 20 +++------------ Firmware/MotorControl/controller.cpp | 2 +- Firmware/MotorControl/encoder.cpp | 27 +++++++++++++------- Firmware/MotorControl/encoder.hpp | 2 ++ Firmware/MotorControl/motor.cpp | 19 ++++++++------ Firmware/MotorControl/motor.hpp | 5 ++-- Firmware/README.md | 2 +- tools/odrive/enums.py | 2 +- tools/odrive/tests.py | 2 +- 11 files changed, 68 insertions(+), 53 deletions(-) diff --git a/Firmware/.vscode/c_cpp_properties.json b/Firmware/.vscode/c_cpp_properties.json index eab99cd5..7f70edfe 100644 --- a/Firmware/.vscode/c_cpp_properties.json +++ b/Firmware/.vscode/c_cpp_properties.json @@ -126,8 +126,10 @@ ], "limitSymbolsToIncludedHeaders": true, "databaseFilename": "" - } + }, + "cStandard": "c11", + "cppStandard": "c++17" } ], - "version": 3 + "version": 4 } \ No newline at end of file diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 7ddce582..c1576a82 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -92,16 +92,26 @@ void Axis::set_step_dir_enabled(bool enable) { } } -// @brief Returns true if everything is ok. -// Sets error and returns false otherwise. +// @brief Do axis level checks and call subcomponent do_checks +// Returns true if everything is ok. bool Axis::do_checks() { - if (!motor_.do_checks()) - return error_ |= ERROR_MOTOR_FAILED, false; + if (!brake_resistor_armed) + error_ |= ERROR_BRAKE_RESISTOR_DISARMED; + 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_MOTOR_DISARMED; if (!(vbus_voltage >= board_config.dc_bus_undervoltage_trip_level)) - return error_ |= ERROR_DC_BUS_UNDER_VOLTAGE, false; + error_ |= ERROR_DC_BUS_UNDER_VOLTAGE; if (!(vbus_voltage <= board_config.dc_bus_overvoltage_trip_level)) - return error_ |= ERROR_DC_BUS_OVER_VOLTAGE, false; - return true; + error_ |= ERROR_DC_BUS_OVER_VOLTAGE; + + // Sub-components should use set_error which will propegate to this error_ + motor_.do_checks(); + encoder_.do_checks(); + // sensorless_estimator_.do_checks(); + // controller_.do_checks(); + + return error_ == ERROR_NONE; } bool Axis::run_sensorless_spin_up() { @@ -115,7 +125,7 @@ bool Axis::run_sensorless_spin_up() { return error_ |= ERROR_MOTOR_FAILED, false; return x < 1.0f; }); - if (error_ != ERROR_NO_ERROR) + if (error_ != ERROR_NONE) return false; // Late Spin-up: accelerate @@ -129,7 +139,7 @@ bool Axis::run_sensorless_spin_up() { return error_ |= ERROR_MOTOR_FAILED, false; return vel < config_.spin_up_target_vel; }); - return error_ == ERROR_NO_ERROR; + return error_ == ERROR_NONE; } // Note run_sensorless_control_loop and run_closed_loop_control_loop are very similar and differ only in where we get the estimate from. @@ -152,7 +162,7 @@ bool Axis::run_sensorless_control_loop() { return true; }); set_step_dir_enabled(false); - return error_ == ERROR_NO_ERROR; + return error_ == ERROR_NONE; } bool Axis::run_closed_loop_control_loop() { @@ -171,7 +181,7 @@ bool Axis::run_closed_loop_control_loop() { return true; }); set_step_dir_enabled(false); - return error_ == ERROR_NO_ERROR; + return error_ == ERROR_NONE; } bool Axis::run_idle_loop() { @@ -183,7 +193,7 @@ bool Axis::run_idle_loop() { encoder_.update(nullptr, nullptr, nullptr); return true; }); - return error_ == ERROR_NO_ERROR; + return error_ == ERROR_NONE; } // Infinite loop that does calibration and enters main control loop as appropriate diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index c39ee5f7..e558c388 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -42,7 +42,7 @@ struct AxisConfig_t { class Axis { public: enum Error_t { - ERROR_NO_ERROR = 0x00, + ERROR_NONE = 0x00, ERROR_INVALID_STATE = 0x01, // void run_control_loop(const T& update_handler) { while (requested_state_ == AXIS_STATE_UNDEFINED) { - if (!brake_resistor_armed) { - error_ |= ERROR_BRAKE_RESISTOR_DISARMED; - 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_MOTOR_DISARMED; - break; - } - if (motor_.error_ != Motor::ERROR_NO_ERROR) { - error_ |= ERROR_MOTOR_FAILED; - break; - } - - if (!do_checks()) // error set during function call + if (!do_checks()) // look for errors at axis level and also all subcomponents break; // Run main loop function, defer quitting for after wait @@ -160,7 +146,7 @@ public: volatile bool thread_id_valid_ = false; // variables exposed on protocol - Error_t error_ = ERROR_NO_ERROR; + Error_t error_ = ERROR_NONE; bool enable_step_dir_ = false; // auto enabled after calibration, based on config.enable_step_dir AxisState_t requested_state_ = AXIS_STATE_STARTUP_SEQUENCE; AxisState_t task_chain_[10] = { AXIS_STATE_UNDEFINED }; diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index 7cc55894..43bb206b 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -46,7 +46,7 @@ void Controller::set_current_setpoint(float current_setpoint) { void Controller::start_anticogging_calibration() { // Ensure the cogging map was correctly allocated earlier and that the motor is capable of calibrating - if (anticogging_.cogging_map != NULL && axis_->error_ == Axis::ERROR_NO_ERROR) { + if (anticogging_.cogging_map != NULL && axis_->error_ == Axis::ERROR_NONE) { anticogging_.calib_anticogging = true; } } diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 5bf57c1c..55de1bc8 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -31,6 +31,15 @@ void Encoder::setup() { enc_index_cb_wrapper, this); } +void Encoder::set_error(Encoder::Error_t error) { + error_ |= error; + axis_->error_ |= Axis::ERROR_MOTOR_FAILED; +} + +bool Encoder::do_checks(){ + return error_ == ERROR_NONE; +} + //-------------------- // Hardware Dependent //-------------------- @@ -112,7 +121,7 @@ bool Encoder::run_index_search() { // continue until the index is found return !index_found_; }); - return axis_->error_ != Axis::ERROR_NO_ERROR; + return axis_->error_ != Axis::ERROR_NONE; } // @brief Turns the motor in one direction for a bit and then in the other @@ -148,7 +157,7 @@ bool Encoder::run_offset_calibration() { axis_->motor_.log_timing(Motor::TIMING_LOG_ENC_CALIB); return ++i < start_lock_duration * current_meas_hz; }); - if (axis_->error_ != Axis::ERROR_NO_ERROR) + if (axis_->error_ != Axis::ERROR_NONE) return false; int32_t init_enc_val = shadow_count_; @@ -170,7 +179,7 @@ bool Encoder::run_offset_calibration() { return ++i < num_steps; }); - if (axis_->error_ != Axis::ERROR_NO_ERROR) + if (axis_->error_ != Axis::ERROR_NONE) return false; //TODO avoid recomputing elec_rad_per_enc every time @@ -179,7 +188,7 @@ bool Encoder::run_offset_calibration() { float actual_encoder_delta_abs = fabsf(shadow_count_-init_enc_val); if(fabsf(actual_encoder_delta_abs - expected_encoder_delta)/expected_encoder_delta > config_.calib_range) { - error_ |= ERROR_CPR_OUT_OF_RANGE; + set_error(ERROR_CPR_OUT_OF_RANGE); return false; } // check direction @@ -191,7 +200,7 @@ bool Encoder::run_offset_calibration() { axis_->motor_.config_.direction = -1; } else { // Encoder response error - error_ |= ERROR_RESPONSE; + set_error(ERROR_RESPONSE); return false; } @@ -211,7 +220,7 @@ bool Encoder::run_offset_calibration() { return ++i < num_steps; }); - if (axis_->error_ != Axis::ERROR_NO_ERROR) + if (axis_->error_ != Axis::ERROR_NONE) return false; offset_ = encvaluesum / (num_steps * 2); @@ -238,7 +247,7 @@ static bool decode_hall(uint8_t hall_state, int32_t* hall_cnt) { 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; + set_error(ERROR_NUMERICAL); return false; } @@ -260,13 +269,13 @@ bool Encoder::update(float* pos_estimate, float* vel_estimate, float* phase_outp if (delta_enc > 3) delta_enc -= 6; } else { - error_ |= ERROR_ILLEGAL_HALL_STATE; + set_error(ERROR_ILLEGAL_HALL_STATE); return false; } } break; default: { - error_ |= ERROR_UNSUPPORTED_ENCODER_MODE; + set_error(ERROR_UNSUPPORTED_ENCODER_MODE); return 0; } break; } diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index fafd0cc5..fe3d717e 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -41,6 +41,8 @@ public: Config_t& config); void setup(); + void set_error(Error_t error); + bool do_checks(); void enc_index_cb(); diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index dad64bc5..2b0a9daa 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -125,9 +125,14 @@ bool Motor::check_DRV_fault() { return true; } +void Motor::set_error(Motor::Error_t error){ + error_ |= error; + axis_->error_ |= Axis::ERROR_MOTOR_FAILED; +} + bool Motor::do_checks() { if (!check_DRV_fault()) { - error_ |= ERROR_DRV_FAULT; + set_error(ERROR_DRV_FAULT); return false; } return true; @@ -172,7 +177,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 set_error(ERROR_PHASE_RESISTANCE_OUT_OF_RANGE), false; // Test voltage along phase A if (!enqueue_voltage_timings(test_voltage, 0.0f)) @@ -181,7 +186,7 @@ bool Motor::measure_phase_resistance(float test_current, float max_voltage) { return ++i < num_test_cycles; }); - if (axis_->error_ != Axis::ERROR_NO_ERROR) + if (axis_->error_ != Axis::ERROR_NONE) return false; //// De-energize motor @@ -212,7 +217,7 @@ bool Motor::measure_phase_inductance(float voltage_low, float voltage_high) { return ++t < (num_cycles << 1); }); - if (axis_->error_ != Axis::ERROR_NO_ERROR) + if (axis_->error_ != Axis::ERROR_NONE) return false; //// De-energize motor @@ -228,7 +233,7 @@ 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 set_error(ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE), false; return true; } @@ -255,7 +260,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 set_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); @@ -363,7 +368,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; + set_error(ERROR_NOT_IMPLEMENTED_MOTOR_TYPE); return false; } return true; diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index b369460e..d33a44bb 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -54,7 +54,7 @@ typedef struct { class Motor { public: enum Error_t { - ERROR_NO_ERROR = 0, + ERROR_NONE = 0, ERROR_PHASE_RESISTANCE_OUT_OF_RANGE = 0x0001, ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE = 0x0002, ERROR_ADC_FAILED = 0x0004, @@ -102,6 +102,7 @@ public: void update_current_controller_gains(); void DRV8301_setup(); bool check_DRV_fault(); + void set_error(Error_t error); bool do_checks(); void log_timing(TimingLog_t log_idx); float phase_current_from_adcval(uint32_t ADCValue); @@ -133,7 +134,7 @@ public: uint16_t timing_log_[TIMING_LOG_NUM_SLOTS] = { 0 }; // variables exposed on protocol - Error_t error_ = ERROR_NO_ERROR; + Error_t error_ = ERROR_NONE; // Do not write to this variable directly! // It is for exclusive use by the safety_critical_... functions. ArmedState_t armed_state_ = ARMED_STATE_DISARMED; diff --git a/Firmware/README.md b/Firmware/README.md index ad315ac0..d101dce3 100644 --- a/Firmware/README.md +++ b/Firmware/README.md @@ -272,7 +272,7 @@ If you have an encoder with an index (Z) signal, you may avoid having to do the

The error nummber corresponds to the following: -0. `ERROR_NO_ERROR` +0. `ERROR_NONE` 1. `ERROR_PHASE_RESISTANCE_TIMING` 2. `ERROR_PHASE_RESISTANCE_MEASUREMENT_TIMEOUT` 3. `ERROR_PHASE_RESISTANCE_OUT_OF_RANGE` diff --git a/tools/odrive/enums.py b/tools/odrive/enums.py index 52b1e610..324a03a9 100644 --- a/tools/odrive/enums.py +++ b/tools/odrive/enums.py @@ -11,7 +11,7 @@ AXIS_STATE_ENCODER_INDEX_SEARCH = 6 AXIS_STATE_ENCODER_OFFSET_CALIBRATION = 7 AXIS_STATE_CLOSED_LOOP_CONTROL = 8 -AXIS_ERROR_NO_ERROR = 0 +AXIS_ERROR_NONE = 0 AXIS_ERROR_INVALID_STATE = 1 #AXIS_ERROR_DC_BUS_UNDER_VOLTAGE = 2 #AXIS_ERROR_DC_BUS_OVER_VOLTAGE = 3 diff --git a/tools/odrive/tests.py b/tools/odrive/tests.py index 43ef182b..6fe5df1b 100644 --- a/tools/odrive/tests.py +++ b/tools/odrive/tests.py @@ -111,7 +111,7 @@ def request_state(axis_ctx: AxisTestContext, state, expect_success=True): else: test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_IDLE) test_assert_eq(axis_ctx.handle.error, AXIS_ERROR_INVALID_STATE) - axis_ctx.handle.error = AXIS_ERROR_NO_ERROR # reset error + axis_ctx.handle.error = AXIS_ERROR_NONE # reset error def set_limits(axis_ctx: AxisTestContext, logger, vel_limit=20000, current_limit=10): """