diff --git a/control_loop/bldc/foc/brushless_foc_control_loop.cpp b/control_loop/bldc/foc/brushless_foc_control_loop.cpp index bb3427e..b83c349 100644 --- a/control_loop/bldc/foc/brushless_foc_control_loop.cpp +++ b/control_loop/bldc/foc/brushless_foc_control_loop.cpp @@ -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; diff --git a/hwbridge/3phase/phase_inductance_estimator.hpp b/hwbridge/3phase/phase_inductance_estimator.hpp index 2e06375..2972280 100644 --- a/hwbridge/3phase/phase_inductance_estimator.hpp +++ b/hwbridge/3phase/phase_inductance_estimator.hpp @@ -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 */ /** diff --git a/hwbridge/3phase/phase_resistance_estimator.hpp b/hwbridge/3phase/phase_resistance_estimator.hpp index 88d1d77..b1778fb 100644 --- a/hwbridge/3phase/phase_resistance_estimator.hpp +++ b/hwbridge/3phase/phase_resistance_estimator.hpp @@ -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 */ /** diff --git a/test/brushless_control_loop_test.cpp b/test/brushless_control_loop_test.cpp index 19fe554..dccc717 100644 --- a/test/brushless_control_loop_test.cpp +++ b/test/brushless_control_loop_test.cpp @@ -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 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 \ No newline at end of file