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

turn error enums into flags

Browse Source
This commit is contained in:
Samuel Sadok
2018-04-06 13:12:39 -07:00
parent 99c5649105
commit ae9a208f1f
11 changed files with 85 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Firmware/MotorControl/axis.cpp
+13 -12
View File
@@ -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, &current_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, &current_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;
}
Firmware/MotorControl/axis.hpp
+20 -17
View File
@@ -43,17 +43,17 @@ struct AxisConfig_t {
class Axis {
public:
enum Error_t {
ERROR_NO_ERROR = 0,
ERROR_INVALID_STATE = 1, //<! an invalid state was requested
ERROR_DC_BUS_UNDER_VOLTAGE = 2,
ERROR_DC_BUS_OVER_VOLTAGE = 3,
ERROR_CURRENT_MEASUREMENT_TIMEOUT = 4,
ERROR_CONTROL_LOOP_TIMEOUT = 5,
ERROR_MOTOR_FAILED = 6,
ERROR_SENSORLESS_ESTIMATOR_FAILED = 7,
ERROR_ENCODER_FAILED = 8,
ERROR_CONTROLLER_FAILED = 9,
ERROR_POS_CTRL_DURING_SENSORLESS = 10,
ERROR_NO_ERROR = 0x00,
ERROR_INVALID_STATE = 0x01, //<! an invalid state was requested
ERROR_DC_BUS_UNDER_VOLTAGE = 0x02,
ERROR_DC_BUS_OVER_VOLTAGE = 0x04,
ERROR_CURRENT_MEASUREMENT_TIMEOUT = 0x08,
ERROR_MOTOR_DISARMED = 0x10, //<! the motor was unexpectedly disarmed
ERROR_MOTOR_FAILED = 0x20,
ERROR_SENSORLESS_ESTIMATOR_FAILED = 0x40,
ERROR_ENCODER_FAILED = 0x80,
ERROR_CONTROLLER_FAILED = 0x100,
ERROR_POS_CTRL_DURING_SENSORLESS = 0x200,
};
enum thread_signals {
@@ -102,13 +102,13 @@ public:
template<typename T>
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 */
Firmware/MotorControl/encoder.cpp
+3 -3
View File
@@ -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;
}
Firmware/MotorControl/encoder.hpp
+6 -4
View File
@@ -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
Firmware/MotorControl/low_level.cpp
+9 -3
View File
@@ -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(
Firmware/MotorControl/low_level.h
+1 -1
View File
@@ -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();
Firmware/MotorControl/motor.cpp
+6 -8
View File
@@ -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;
Firmware/MotorControl/motor.hpp
+11 -8
View File
@@ -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
Firmware/MotorControl/odrive_main.hpp
+11
View File
@@ -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<ENUMTYPE>(static_cast<std::underlying_type_t<ENUMTYPE>>(a) | static_cast<std::underlying_type_t<ENUMTYPE>>(b)); } \
inline ENUMTYPE operator & (ENUMTYPE a, ENUMTYPE b) { return static_cast<ENUMTYPE>(static_cast<std::underlying_type_t<ENUMTYPE>>(a) & static_cast<std::underlying_type_t<ENUMTYPE>>(b)); } \
inline ENUMTYPE operator ^ (ENUMTYPE a, ENUMTYPE b) { return static_cast<ENUMTYPE>(static_cast<std::underlying_type_t<ENUMTYPE>>(a) ^ static_cast<std::underlying_type_t<ENUMTYPE>>(b)); } \
inline ENUMTYPE &operator |= (ENUMTYPE &a, ENUMTYPE b) { return reinterpret_cast<ENUMTYPE&>(reinterpret_cast<std::underlying_type_t<ENUMTYPE>&>(a) |= static_cast<std::underlying_type_t<ENUMTYPE>>(b)); } \
inline ENUMTYPE &operator &= (ENUMTYPE &a, ENUMTYPE b) { return reinterpret_cast<ENUMTYPE&>(reinterpret_cast<std::underlying_type_t<ENUMTYPE>&>(a) &= static_cast<std::underlying_type_t<ENUMTYPE>>(b)); } \
inline ENUMTYPE &operator ^= (ENUMTYPE &a, ENUMTYPE b) { return reinterpret_cast<ENUMTYPE&>(reinterpret_cast<std::underlying_type_t<ENUMTYPE>&>(a) ^= static_cast<std::underlying_type_t<ENUMTYPE>>(b)); } \
inline ENUMTYPE operator ~ (ENUMTYPE a) { return static_cast<ENUMTYPE>(~static_cast<std::underlying_type_t<ENUMTYPE>>(a)); }
// ODrive specific includes
#include <protocol.hpp>
Firmware/MotorControl/sensorless_estimator.cpp
+1 -1
View File
@@ -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;
}
Firmware/MotorControl/sensorless_estimator.hpp
+4 -2
View File
@@ -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 */