[Bridge] Add 3 phase inductance estimator

CMakeLists.txt

@@ -23,6 +23,9 @@ add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/libs/googletest ${CMAKE_CURRENT_BIN
 # Glob recurse the sources inside control_loop/
 file(GLOB_RECURSE CONTROL_LOOP_SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/control_loop/*.cpp)
 
+# Glob recurse the sources inside hwbridge/3phase/
+file(GLOB_RECURSE HWBRIDGE_3PHASE_SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/hwbridge/3phase/*.cpp)
+
 # Glob recurse the sources inside utils/
 file(GLOB_RECURSE UTILS_SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/util/*.cpp)
 
@@ -51,7 +54,7 @@ include_directories(${CMAKE_CURRENT_SOURCE_DIR}/util/math)
 # Mocks
 include_directories(${CMAKE_CURRENT_SOURCE_DIR}/test/mocks)
 
-add_library(MAIN_SOURCES ${CONTROL_LOOP_SOURCES} ${UTILS_SOURCES})
+add_library(MAIN_SOURCES ${CONTROL_LOOP_SOURCES} ${UTILS_SOURCES} ${HWBRIDGE_3PHASE_SOURCES})
 # Set pedantic and pedantic-errors flags for MAIN_SOURCES
 target_compile_options(MAIN_SOURCES PRIVATE -pedantic -pedantic-errors)
 

hwbridge/3phase/bridge_3phase.hpp

@@ -21,6 +21,15 @@ class Bridge3Phase {
      */
     class phase_command_t {
        public:
+        phase_command_t() = default;
+        /**
+         * @brief Construct a phase command
+         * @param duty_cycle_high_side Duty cycle for the high side PWM channel assuming complementary PWM
+         * @param invert_low_side Invert the low side PWM channel (used to High-Z the bridge output)
+         * @note The duty cycle is between 0.0f and 1.0f, where 0.5 is 50% duty cycle and represents 0V (assumes complementary
+         */
+        phase_command_t(float duty_cycle_high_side, bool invert_low_side)
+            : duty_cycle_high_side(duty_cycle_high_side), invert_low_side(invert_low_side) {}
         /**
          * @brief Duty cycle for the high side PWM channel assuming complementary PWM
          * @note The duty cycle is between 0.0f and 1.0f, where 0.5 is 50% duty cycle and represents 0V (assumes complementary

hwbridge/3phase/phase_inductance_estimator.cpp

@@ -0,0 +1,117 @@
+#include "phase_inductance_estimator.hpp"
+
+#include "math_util.hpp"
+
+namespace hwbridge {
+PhaseInductanceEstimatorController::Result PhaseInductanceEstimatorController::run_phase_inductance_estimator(
+    PhaseInductanceEstimatorController::Input input) {
+    Result result;
+    // Get the current time
+    const utime_t current_time = clock_.get_time_us();
+    state_ = get_desired_state(current_time, input);
+
+    const hwbridge::Bridge3Phase::phase_command_t brake_command = {0.0f, true};
+    const hwbridge::Bridge3Phase::phase_command_t high_z_command = {0.0f, false};
+    switch (state_) {
+        case State::ERROR:
+        case State::NOT_STARTED: {
+            result.phase_commands[0] = high_z_command;
+            result.phase_commands[1] = high_z_command;
+            result.phase_commands[2] = high_z_command;
+            result.is_phase_inductance_valid = false;
+            break;
+        }
+        case State::BRAKE_ROTOR: {
+            if (brake_start_time_ == 0) {
+                brake_start_time_ = current_time;
+            }
+            result.phase_commands[0] = brake_command;
+            result.phase_commands[1] = brake_command;
+            result.phase_commands[2] = brake_command;
+            break;
+        }
+        case State::MEASUREMENT_IN_PROGRESS: {
+            const hwbridge::Bridge3Phase::phase_command_t square_wave_command = {1.0f, true};
+            if (measurement_start_time_ == 0) {
+                measurement_start_time_ = current_time;
+            }
+            result.phase_commands[0] = brake_command;
+            result.phase_commands[1] = square_wave_command;
+            result.phase_commands[2] = square_wave_command;
+            break;
+        }
+        case State::ESTIMATE_COMPLETE: {
+            // High Z the bridge
+            result.phase_commands[0] = high_z_command;
+            result.phase_commands[1] = high_z_command;
+            result.phase_commands[2] = high_z_command;
+
+            // Calculate the phase inductance
+            // TODO: Perhaps take an average of the bus voltage during the measurement period? But it's likely super fast anyways
+            const float bus_voltage = input.bus_voltage;
+            const float current = input.phase_currents.u;
+            // V = 3/2*L_s * di/dt
+            // L_s = 2/3 * V / di/dt
+            // Note: the L_s is the phase inductance in a 3-phase system. When we measure the current in the singular phase that
+            // is grounded, because we short 2 of the phases to Vbus, we end up with the resistance and inductance measurement of
+            // the combined system, which is 3/2 of the phase inductance and resistance. So we need to divide by 3/2 to get the
+            // phase inductance.
+            const float dt = clock_.get_dt_s(current_time, measurement_start_time_);
+            const float di_dt = current / dt;
+            result.phase_inductance = (2.0f / 3.0f) * bus_voltage / di_dt;
+            result.is_phase_inductance_valid = true;
+            break;
+        }
+
+        default:
+            break;
+    }
+
+    result.state = state_;
+    return result;
+}
+
+PhaseInductanceEstimatorController::State PhaseInductanceEstimatorController::get_desired_state(utime_t current_time,
+                                                                                                Input input) const {
+    State ret = state_;
+    switch (state_) {
+        case State::NOT_STARTED:
+            if (params_.measurement_duration == 0) {
+                ret = State::ERROR;
+            } else if (params_.brake_duration > 0) {
+                ret = State::BRAKE_ROTOR;
+            } else {
+                ret = State::MEASUREMENT_IN_PROGRESS;
+            }
+            break;
+        case State::BRAKE_ROTOR: {
+            const utime_t brake_end_time = brake_start_time_ + params_.brake_duration;
+            if (current_time >= brake_end_time) {
+                ret = State::MEASUREMENT_IN_PROGRESS;
+            }
+            break;
+        }
+        case State::MEASUREMENT_IN_PROGRESS: {
+            const utime_t measurement_end_time = measurement_start_time_ + params_.measurement_duration;
+            if (current_time >= measurement_end_time) {
+                const bool is_current_nonzero = (math::float_equals(input.phase_currents.u, 0.0f) == false);
+                const bool is_bus_voltage_nonzero = (math::float_equals(input.bus_voltage, 0.0f) == false);
+                if (is_current_nonzero && is_bus_voltage_nonzero) {
+                    ret = State::ESTIMATE_COMPLETE;
+                } else {
+                    ret = State::ERROR;
+                }
+            }
+            break;
+        }
+        case State::ERROR:
+        case State::ESTIMATE_COMPLETE:
+        default:
+            // Do not change the state
+            ret = state_;
+            break;
+    }
+
+    return ret;
+}
+}  // namespace hwbridge

hwbridge/3phase/phase_inductance_estimator.hpp

@@ -0,0 +1,97 @@
+#ifndef PHASE_INDUCTANCE_ESTIMATOR_HPP
+#define PHASE_INDUCTANCE_ESTIMATOR_HPP
+#include "bridge_3phase.hpp"
+#include "hal_clock.hpp"
+
+namespace hwbridge {
+
+/**
+ * @brief Control Law for estimating the phase inductance of a 3 phase motor
+ * @todo Make the axis alignment of the phase inductance estimator configurable (defaults to U)
+ */
+class PhaseInductanceEstimatorController {
+   public:
+    /// @brief Parameters for the Phase Inductance Estimator
+    class Params {
+       public:
+        /// @brief The duration of the time the rotor phases should be grounded (us)
+        utime_t brake_duration;
+        /// @brief The duration of the time the rotor phases should be driven with a square wave (us)
+        utime_t measurement_duration;
+    };
+
+    /**
+     * @brief State of the Phase Inductance Estimator
+     */
+    enum class State {
+        /// @brief The Phase Inductance Estimator has not been run
+        NOT_STARTED,
+        /// @brief Rotor dridge is commanded to ground all phases
+        BRAKE_ROTOR,
+        /// @brief Rotor bridge is commanded with square wave to estimate phase inductance
+        MEASUREMENT_IN_PROGRESS,
+        /// @brief An error has occured during the measurement
+        ERROR,
+        /// @brief The measurement has completed successfully
+        ESTIMATE_COMPLETE
+    };
+
+    /// @brief Inputs to the Phase Inductance Estimator
+    class Input {
+       public:
+        /// @brief The current phase currents (A)
+        hwbridge::Bridge3Phase::phase_current_t phase_currents;
+        /// @brief The bus voltage (V)
+        float bus_voltage = 0.0f;
+    };
+
+    /// @brief Result of the Phase Inductance Estimator
+    class Result {
+       public:
+        /// @brief The estimated phase inductance (H)
+        float phase_inductance = 0.0f;
+        /// @brief The state of the Phase Inductance Estimator
+        PhaseInductanceEstimatorController::State state = PhaseInductanceEstimatorController::State::NOT_STARTED;
+        /// @brief Whether or not the phase inductance is valid
+        bool is_phase_inductance_valid = false;
+
+        /// @brief The commands to the rotor bridge
+        hwbridge::Bridge3Phase::phase_command_t phase_commands[3];
+    };
+
+    /**
+     * @brief Construct a new Phase Inductance Estimator Controller object
+     * @param clock Reference to the HAL_CLOCK object used to get the current time
+     * @param params Parameters for the Phase Inductance Estimator
+     */
+    PhaseInductanceEstimatorController(basilisk_hal::HAL_CLOCK& clock, Params params) : params_(params), clock_(clock) {}
+
+    /**
+     * @brief Run the Phase Inductance Estimator
+     * @param input The input to the Phase Inductance Estimator
+     * @return The result of the Phase Inductance Estimator
+     */
+    Result run_phase_inductance_estimator(Input input);
+
+   protected:
+    /*! \cond PRIVATE */
+    /**
+     * @brief The desired state of the Phase Inductance Estimator
+     * @param current_time The current time
+     * @param input The input to the Phase Inductance Estimator
+     * @return The desired state of the Phase Inductance Estimator
+     */
+    State get_desired_state(utime_t current_time, Input input) const;
+
+    Params params_;
+    State state_ = State::NOT_STARTED;
+    basilisk_hal::HAL_CLOCK& clock_;
+
+    utime_t brake_start_time_ = 0;
+    utime_t measurement_start_time_ = 0;
+    /*! \endcond */
+};
+
+}  // namespace hwbridge
+
+#endif  // PHASE_INDUCTANCE_ESTIMATOR_HPP