Mirrors_Framework/ODrive
1
0
Fork 0
mirror of https://github.com/odriverobotics/ODrive.git synced 2026-09-22 08:04:07 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Make subcomponent errors to always set relevant axis error; use do_checks and set_error

Browse Source
This commit is contained in:
Oskar Weigl
2018-05-10 06:56:13 -07:00
parent 08758c848d
commit a94096b64d
11 changed files with 68 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Firmware/.vscode/c_cpp_properties.json
Vendored
+4 -2
View File
@@ -126,8 +126,10 @@
],
"limitSymbolsToIncludedHeaders": true,
"databaseFilename": ""
}
},
"cStandard": "c11",
"cppStandard": "c++17"
}
],
"version": 3
"version": 4
}
Firmware/MotorControl/axis.cpp
+22 -12
View File
@@ -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
Firmware/MotorControl/axis.hpp
+3 -17
View File
@@ -42,7 +42,7 @@ struct AxisConfig_t {
class Axis {
public:
enum Error_t {
ERROR_NO_ERROR = 0x00,
ERROR_NONE = 0x00,
ERROR_INVALID_STATE = 0x01, //<! an invalid state was requested
ERROR_DC_BUS_UNDER_VOLTAGE = 0x02,
ERROR_DC_BUS_OVER_VOLTAGE = 0x04,
@@ -102,21 +102,7 @@ public:
template<typename T>
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 };
Firmware/MotorControl/controller.cpp
+1 -1
View File
@@ -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;
}
}
Firmware/MotorControl/encoder.cpp
+18 -9
View File
@@ -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;
}
Firmware/MotorControl/encoder.hpp
+2
View File
@@ -41,6 +41,8 @@ public:
Config_t& config);
void setup();
void set_error(Error_t error);
bool do_checks();
void enc_index_cb();
Firmware/MotorControl/motor.cpp
+12 -7
View File
@@ -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;
Firmware/MotorControl/motor.hpp
+3 -2
View File
@@ -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;
Firmware/README.md
+1 -1
View File
@@ -272,7 +272,7 @@ If you have an encoder with an index (Z) signal, you may avoid having to do the
<br><br>
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`
tools/odrive/enums.py
+1 -1
View File
@@ -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
tools/odrive/tests.py
+1 -1
View File
@@ -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):
"""