[BLDC] Prevent control loop from going outside of stop if there is an…

… error - Also added state hooks to the phase param estimators
sahil-kale · Jan 8, 2024 · 729b4ae · 729b4ae
1 parent af3da86
commit 729b4ae
Show file tree

Hide file tree

Showing 4 changed files with 33 additions and 1 deletion.
diff --git 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
@@ -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
@@ -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
@@ -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