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):
"""