Skip to content

Commit

Permalink
[Bridge] Add 3 phase inductance estimator
Browse files Browse the repository at this point in the history
  • Loading branch information
sahil-kale committed Jan 6, 2024
1 parent 140a064 commit f3ecdb9
Show file tree
Hide file tree
Showing 5 changed files with 522 additions and 1 deletion.
5 changes: 4 additions & 1 deletion CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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)

Expand Down Expand Up @@ -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)

Expand Down
9 changes: 9 additions & 0 deletions hwbridge/3phase/bridge_3phase.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
117 changes: 117 additions & 0 deletions hwbridge/3phase/phase_inductance_estimator.cpp
Original file line number Diff line number Diff line change
@@ -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
97 changes: 97 additions & 0 deletions hwbridge/3phase/phase_inductance_estimator.hpp
Original file line number Diff line number Diff line change
@@ -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
Loading

0 comments on commit f3ecdb9

Please sign in to comment.