Skip to content

Commit

Permalink
[BLDC] Prevent control loop from going outside of stop if there is an…
Browse files Browse the repository at this point in the history
… error

- Also added state hooks to the phase param estimators
  • Loading branch information
sahil-kale committed Jan 8, 2024
1 parent af3da86 commit 729b4ae
Show file tree
Hide file tree
Showing 4 changed files with 33 additions and 1 deletion.
3 changes: 2 additions & 1 deletion control_loop/bldc/foc/brushless_foc_control_loop.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,8 @@ BrushlessFOCControlLoop::State BrushlessFOCControlLoop::get_desired_state(float
const bool i_q_reference_is_zero = math::float_equals(i_q_reference, 0.0f);
switch (current_state) {
case State::STOP: {
if (i_q_reference_is_zero == false) {
const bool has_error = (status == ControlLoop::ControlLoopBaseStatus::ERROR);
if ((i_q_reference_is_zero == false) && (has_error == false)) {
// We want to start the motor
if (params.foc_params.phase_resistance_valid && params.foc_params.phase_inductance_valid) {
desired_state = State::RUN;
Expand Down
6 changes: 6 additions & 0 deletions hwbridge/3phase/phase_inductance_estimator.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -79,6 +79,12 @@ class PhaseInductanceEstimatorController {
*/
Result run_phase_inductance_estimator(Input input);

/**
* @brief Get the state of the Phase Inductance Estimator
* @return The state of the Phase Inductance Estimator
*/
State get_state() const { return state_; }

protected:
/*! \cond PRIVATE */
/**
Expand Down
6 changes: 6 additions & 0 deletions hwbridge/3phase/phase_resistance_estimator.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -100,6 +100,12 @@ class PhaseResistanceEstimatorController {
*/
Result run_phase_resistance_estimator(Input input);

/**
* @brief Get the state of the Phase Resistance Estimator
* @return The state of the Phase Resistance Estimator
*/
State get_state() const { return state_; }

protected:
/*! \cond PRIVATE */
/**
Expand Down
19 changes: 19 additions & 0 deletions test/brushless_control_loop_test.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -581,4 +581,23 @@ TEST(BrushlessFOCControlLoopTest, test_desired_state_calibration_to_stop_after_f
EXPECT_EQ(desired_state, BrushlessFOCControlLoop::State::STOP);
}

// Test that if the current state is stop and a reference is given, but the status has an error, the desired state is stop
TEST(BrushlessFOCControlLoopTest, test_desired_state_stop_to_stop_after_error) {
// Create an absolute rotor position estimator
NiceMock<bldc_rotor_estimator::MOCK_ROTOR_ABSOLUTE_SENSOR> rotor_sensor;

// instantiate a brushless foc control loop test class
BrushlessFOCControlLoopTest test_control_loop(rotor_sensor, mock_clock);

BrushlessFOCControlLoop::Params test_params = test_control_loop.test_params_;

// Get the desired state
BrushlessFOCControlLoop::BrushlessFOCControlLoopStatus status = BrushlessFOCControlLoop::BrushlessFOCControlLoopStatus();
status.set_error(BrushlessFOCControlLoop::BrushlessFOCControlLoopError::PHASE_INDUCTANCE_ESTIMATOR_FAILURE, true);
BrushlessFOCControlLoop::State desired_state =
test_control_loop.get_desired_state(0.1, BrushlessFOCControlLoop::State::STOP, test_params, status);

EXPECT_EQ(desired_state, BrushlessFOCControlLoop::State::STOP);
}

} // namespace control_loop

0 comments on commit 729b4ae

Please sign in to comment.