From fe547eb16e39eae372688339841002b31563b2f2 Mon Sep 17 00:00:00 2001 From: Sahil Kale Date: Sun, 7 Jan 2024 15:50:38 +0100 Subject: [PATCH] [BLDC] Integrate phase calibration controllers --- CMakeLists.txt | 5 +- control_loop/bldc/6step/6step_util.cpp | 2 +- .../bldc/foc/brushless_foc_control_loop.cpp | 118 +++++++++++--- .../bldc/foc/brushless_foc_control_loop.hpp | 93 ++++++----- control_loop/bldc/foc/foc_util.cpp | 18 +-- .../bldc/rotor_estimator/rotor_estimator.cpp | 4 +- hal/hal_clock.hpp | 2 +- hwbridge/3phase/bridge_3phase.hpp | 8 + .../3phase/phase_inductance_estimator.cpp | 8 + .../3phase/phase_inductance_estimator.hpp | 6 + .../3phase/phase_resistance_estimator.cpp | 11 ++ .../3phase/phase_resistance_estimator.hpp | 6 + test/brushless_control_loop_test.cpp | 147 ++++++++++++++++-- util/math/math_util.hpp | 6 +- util/pid/pid.cpp | 8 +- 15 files changed, 347 insertions(+), 95 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 100ebda..b2cf0b0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,7 +12,7 @@ set(CMAKE_POSITION_INDEPENDENT_CODE ON) set(BUILD_GMOCK ON) # Add a library with just the sources #enable Werror, Wextra, Wall, pedantic, and pedantic-errors -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Werror -Wextra -Wall -Og -g") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Og -g -Werror -Wextra -Wall") # enable testing enable_testing() @@ -56,7 +56,8 @@ include_directories(${CMAKE_CURRENT_SOURCE_DIR}/test/mocks) 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) +# TODO : enable -Wconversion +target_compile_options(MAIN_SOURCES PRIVATE -pedantic -pedantic-errors -Wfloat-equal -Wredundant-decls -Wswitch-default -pedantic) add_executable(${This} ${TEST_SOURCES}) diff --git a/control_loop/bldc/6step/6step_util.cpp b/control_loop/bldc/6step/6step_util.cpp index cca9426..5f6e59e 100644 --- a/control_loop/bldc/6step/6step_util.cpp +++ b/control_loop/bldc/6step/6step_util.cpp @@ -42,7 +42,7 @@ commutation_step_t determine_commutation_step_from_theta(float electrical_theta) void determine_inverter_duty_cycles_trap(hwbridge::Bridge3Phase::phase_command_t phase_command[3], Bldc6Step::commutation_step_t current_commutation_step, float motor_speed) { // Clamp the motor speed to -1.0f to 1.0f after first abs'ing it - float abs_speed = fabs(motor_speed); + float abs_speed = fabsf(motor_speed); math::clamp(abs_speed, 0.0f, 1.0f); for (int i = 0; i < 3; i++) { if (current_commutation_step.signals[i] == Bldc6Step::CommutationSignal::HIGH) { diff --git a/control_loop/bldc/foc/brushless_foc_control_loop.cpp b/control_loop/bldc/foc/brushless_foc_control_loop.cpp index b065704..bb3427e 100644 --- a/control_loop/bldc/foc/brushless_foc_control_loop.cpp +++ b/control_loop/bldc/foc/brushless_foc_control_loop.cpp @@ -12,7 +12,7 @@ namespace control_loop { using namespace BldcFoc; // Define the init function -void BrushlessFOCControlLoop::init(BrushlessFOCControlLoop::BrushlessFOCControlLoopParams* params) { +void BrushlessFOCControlLoop::init(Params* params) { do { // Initialize the rotor position estimator if (reset_position_estimators(rotor_position_estimators_, num_rotor_position_estimators_) == false) { @@ -23,29 +23,50 @@ void BrushlessFOCControlLoop::init(BrushlessFOCControlLoop::BrushlessFOCControlL status_.reset(); // Set the state to stop - state_ = BrushlessFOCControlLoop::BrushlessFOCControlLoopState::STOP; + state_ = State::STOP; // Set the internal params pointer params_ = params; } while (false); } -BrushlessFOCControlLoop::BrushlessFOCControlLoopState BrushlessFOCControlLoop::get_desired_state( - float i_q_reference, const BrushlessFOCControlLoopState current_state) { - BrushlessFOCControlLoopState desired_state = current_state; +BrushlessFOCControlLoop::State BrushlessFOCControlLoop::get_desired_state(float i_q_reference, const State current_state, + const Params& params, + const BrushlessFOCControlLoopStatus& status) { + State desired_state = current_state; const bool i_q_reference_is_zero = math::float_equals(i_q_reference, 0.0f); switch (current_state) { - case BrushlessFOCControlLoopState::STOP: { - // if the estimator reports that it is valid, then we should start the motor + case State::STOP: { if (i_q_reference_is_zero == false) { - desired_state = BrushlessFOCControlLoopState::RUN; + // We want to start the motor + if (params.foc_params.phase_resistance_valid && params.foc_params.phase_inductance_valid) { + desired_state = State::RUN; + } else { + desired_state = State::CALIBRATION; + } } } break; - case BrushlessFOCControlLoopState::RUN: { + case State::RUN: { if (i_q_reference_is_zero) { - desired_state = BrushlessFOCControlLoopState::STOP; + desired_state = State::STOP; } } break; + case State::CALIBRATION: { + const bool phase_resistance_failed = + status.has_error(BrushlessFOCControlLoopError::PHASE_RESISTANCE_ESTIMATOR_FAILURE); + const bool phase_inductance_failed = + status.has_error(BrushlessFOCControlLoopError::PHASE_INDUCTANCE_ESTIMATOR_FAILURE); + const bool phase_estimation_failed = phase_resistance_failed || phase_inductance_failed; + if (params.foc_params.phase_resistance_valid && params.foc_params.phase_inductance_valid) { + if (i_q_reference_is_zero) { + desired_state = State::STOP; + } else { + desired_state = State::RUN; + } + } else if (phase_estimation_failed) { + desired_state = State::STOP; + } + } default: // Unknown state break; @@ -83,7 +104,7 @@ ControlLoop::ControlLoopBaseStatus BrushlessFOCControlLoop::run_current_control( bridge_, status_, foc_frame_vars_.duty_cycle_result.V_alpha_beta, params_, i_direct_quad_ref); // Get the current state and the desired state - BrushlessFOCControlLoop::BrushlessFOCControlLoopState desired_state = get_desired_state(i_q_reference, state_); + State desired_state = get_desired_state(i_q_reference, state_, *params_, status_); // If the desired state is different from the current state, then we need to transition if (desired_state != state_) { @@ -96,16 +117,18 @@ ControlLoop::ControlLoopBaseStatus BrushlessFOCControlLoop::run_current_control( } // Run the state machine switch (state_) { - case BrushlessFOCControlLoop::BrushlessFOCControlLoopState::STOP: + case State::STOP: break; - case BrushlessFOCControlLoop::BrushlessFOCControlLoopState::RUN: { + case State::RUN: { if (status_ == ControlLoop::ControlLoopBaseStatus::ERROR) { break; } foc_frame_vars_ = foc_controller_.run_foc(foc_inputs, phase_commands); } break; - + case State::CALIBRATION: { + run_calibration(foc_inputs, phase_commands); + } break; default: break; } @@ -132,10 +155,53 @@ ControlLoop::ControlLoopBaseStatus BrushlessFOCControlLoop::run_current_control( return status_.status; } +void BrushlessFOCControlLoop::run_calibration(FOCController::FOCInputs inputs, + hwbridge::Bridge3Phase::phase_command_t phase_commands[3]) { + using namespace hwbridge; + // Turn the FOC alpha/beta inputs into 3 phase current inputs + const math::abc_t i_abc = math::inverse_clarke_transform(inputs.i_alpha_beta.alpha, inputs.i_alpha_beta.beta); + const Bridge3Phase::phase_current_t phase_currents = {i_abc.a, i_abc.b, i_abc.c}; + + if (params_->foc_params.phase_inductance_valid == false) { + // Run the phase inductance estimator + PhaseInductanceEstimatorController::Input input; + input.phase_currents = phase_currents; + input.bus_voltage = inputs.bus_voltage; + PhaseInductanceEstimatorController::Result result = phase_inductance_estimator_.run_phase_inductance_estimator(input); + params_->foc_params.phase_inductance = result.phase_inductance; + params_->foc_params.phase_inductance_valid = result.is_phase_inductance_valid; + + phase_commands[0] = result.phase_commands[0]; + phase_commands[1] = result.phase_commands[1]; + phase_commands[2] = result.phase_commands[2]; + + status_.set_error(BrushlessFOCControlLoopError::PHASE_INDUCTANCE_ESTIMATOR_FAILURE, + result.is_phase_inductance_valid == false); + } else if (params_->foc_params.phase_resistance_valid == false) { + PhaseResistanceEstimatorController::Input input; + // Turn the FOC inputs into + input.phase_currents = phase_currents; + input.bus_voltage = inputs.bus_voltage; + // Run the phase resistance estimator + PhaseResistanceEstimatorController::Result result = phase_resistance_estimator_.run_phase_resistance_estimator(input); + + // Set the phase resistance + params_->foc_params.phase_resistance = result.phase_resistance; + params_->foc_params.phase_resistance_valid = result.is_phase_resistance_valid; + + phase_commands[0] = result.phase_commands[0]; + phase_commands[1] = result.phase_commands[1]; + phase_commands[2] = result.phase_commands[2]; + + status_.set_error(BrushlessFOCControlLoopError::PHASE_RESISTANCE_ESTIMATOR_FAILURE, + result.is_phase_resistance_valid == false); + } +} + void BrushlessFOCControlLoop::update_rotor_position_estimator( bldc_rotor_estimator::ElectricalRotorPosEstimator::EstimatorInputs& estimator_inputs, utime_t current_time_us, - hwbridge::Bridge3Phase::phase_current_t phase_currents, const BrushlessFOCControlLoop::BrushlessFOCControlLoopParams* params, - math::alpha_beta_t V_alpha_beta, BrushlessFOCControlLoopStatus& status, float& theta, + hwbridge::Bridge3Phase::phase_current_t phase_currents, const Params* params, math::alpha_beta_t V_alpha_beta, + BrushlessFOCControlLoopStatus& status, float& theta, bldc_rotor_estimator::ElectricalRotorPosEstimator* rotor_position_estimators[], size_t num_rotor_position_estimators) { do { estimator_inputs.time = current_time_us; @@ -183,17 +249,15 @@ void BrushlessFOCControlLoop::update_rotor_position_estimator( } while (false); } -void BrushlessFOCControlLoop::exit_state(const BrushlessFOCControlLoopState& current_state, - const BrushlessFOCControlLoopState& desired_state) { +void BrushlessFOCControlLoop::exit_state(const State& current_state, const State& desired_state) { IGNORE(current_state); IGNORE(desired_state); } -void BrushlessFOCControlLoop::enter_state(const BrushlessFOCControlLoopState& current_state, - const BrushlessFOCControlLoopState& desired_state) { +void BrushlessFOCControlLoop::enter_state(const State& current_state, const State& desired_state) { IGNORE(current_state); switch (desired_state) { - case BrushlessFOCControlLoop::BrushlessFOCControlLoopState::RUN: { + case State::RUN: { // Set the PI gains const float kp = params_->foc_params.current_control_bandwidth_rad_per_sec * params_->foc_params.phase_inductance; float ki = params_->foc_params.phase_resistance / params_->foc_params.phase_inductance * @@ -214,7 +278,11 @@ void BrushlessFOCControlLoop::enter_state(const BrushlessFOCControlLoopState& cu // Reset the internal state foc_frame_vars_ = FOCController::FOCFrameVars(); } break; - case BrushlessFOCControlLoop::BrushlessFOCControlLoopState::STOP: + case State::CALIBRATION: { + phase_resistance_estimator_.init(params_->phase_resistance_estimator_params); + phase_inductance_estimator_.init(params_->phase_inductance_estimator_params); + } break; + case State::STOP: default: break; } @@ -254,12 +322,12 @@ bool BrushlessFOCControlLoop::reset_position_estimators( FOCController::FOCInputs BrushlessFOCControlLoop::update_foc_inputs( utime_t current_time_us, utime_t last_run_time_us, bldc_rotor_estimator::ElectricalRotorPosEstimator* rotor_position_estimators[], size_t num_rotor_position_estimators, - hwbridge::Bridge3Phase& bridge, BrushlessFOCControlLoopStatus& status, math::alpha_beta_t V_alpha_beta, - const BrushlessFOCControlLoopParams* params, math::direct_quad_t i_direct_quad_ref) { + hwbridge::Bridge3Phase& bridge, BrushlessFOCControlLoopStatus& status, math::alpha_beta_t V_alpha_beta, const Params* params, + math::direct_quad_t i_direct_quad_ref) { FOCController::FOCInputs foc_inputs; do { foc_inputs.timestamp = current_time_us; - foc_inputs.dt = (current_time_us - last_run_time_us) / 1e6f; + foc_inputs.dt = static_cast(current_time_us - last_run_time_us) / 1e6f; hwbridge::Bridge3Phase::phase_current_t phase_currents; app_hal_status_E hal_status = bridge.read_phase_current(phase_currents); diff --git a/control_loop/bldc/foc/brushless_foc_control_loop.hpp b/control_loop/bldc/foc/brushless_foc_control_loop.hpp index 7d8fe6f..03fc05d 100644 --- a/control_loop/bldc/foc/brushless_foc_control_loop.hpp +++ b/control_loop/bldc/foc/brushless_foc_control_loop.hpp @@ -3,14 +3,13 @@ #include -#include - #include "bridge_3phase.hpp" #include "control_loop.hpp" #include "foc_controller.hpp" #include "foc_util.hpp" #include "hal_clock.hpp" -#include "math_foc.hpp" +#include "phase_inductance_estimator.hpp" +#include "phase_resistance_estimator.hpp" #include "pid.hpp" #include "rotor_estimator.hpp" @@ -26,9 +25,13 @@ class BrushlessFOCControlLoop : public ControlLoop { /** * @brief The state of the control loop */ - enum class BrushlessFOCControlLoopState { + enum class State { /// @brief The control loop is stopped (no PWM output) STOP, + /** @brief The control loop is calibrating the motor - it may move slightly during this time + * @details The calibration state is entered when phase resistance or inductance are not valid. + */ + CALIBRATION, /// @brief The control loop is running RUN, }; @@ -41,61 +44,62 @@ class BrushlessFOCControlLoop : public ControlLoop { /** * @brief The bandwidth of the current control loop */ - float current_control_bandwidth_rad_per_sec; - - /** - * @brief The phase resistance of the motor (ohms) - */ - float phase_resistance; - /** - * @brief The phase inductance of the motor (henries) - */ - float phase_inductance; + float current_control_bandwidth_rad_per_sec = 0.0f; + + /// @brief The phase resistance of the motor (ohms) + float phase_resistance = 0.0f; + /// @brief Whether or not the phase resistance is valid + bool phase_resistance_valid = false; + /// @brief The phase inductance of the motor (henries) + float phase_inductance = 0.0f; + /// @brief Whether or not the phase inductance is valid + bool phase_inductance_valid = false; /** * @brief The flux linkage of the permanent magnet (weber) */ - float pm_flux_linkage; + float pm_flux_linkage = 0.0f; - /** - * @brief The timeout period for the foc start (us) - */ - utime_t foc_start_timeout_period_us; /** * @brief Disable the ki term of the current controller * @note Do so if the current controller is unstable in a low bandwidth system, experimentally seems to yield better * results */ - bool disable_ki; + bool disable_ki = false; /** * @brief Converts a generic -1.0 -> 1.0 speed to a iq reference by multiplying by this value by the speed */ - float speed_to_iq_gain; // Converts speed to iq reference + float speed_to_iq_gain = 0.0f; // Converts speed to iq reference /** * @brief The default d current reference (Amps) */ - float i_d_reference_default; + float i_d_reference_default = 0.0f; /** * @brief The PWM control type to use for the FOC control loop */ - BrushlessFocPwmControlType pwm_control_type; + BrushlessFocPwmControlType pwm_control_type = BrushlessFocPwmControlType::SINE; }; /** * @brief The parameters for the control loop */ - class BrushlessFOCControlLoopParams { + class Params { public: /** * @brief The FOC control loop parameters */ BrushlessFocControLoopParams foc_params; /// @brief The open loop full speed theta velocity (rad/s) - float open_loop_theta_velocity; + float open_loop_theta_velocity = 0.0f; /// @brief The magnitude of the direct voltage vector to apply in open loop mode - float open_loop_quadrature_voltage; + float open_loop_quadrature_voltage = 0.0f; + + /// @brief The phase inductance estimator parameters + hwbridge::PhaseInductanceEstimatorController::Params phase_inductance_estimator_params; + /// @brief The phase resistance estimator parameters + hwbridge::PhaseResistanceEstimatorController::Params phase_resistance_estimator_params; }; /** @@ -110,6 +114,10 @@ class BrushlessFOCControlLoop : public ControlLoop { PHASE_COMMAND_FAILURE, /// The phase current read from the bridge failed PHASE_CURRENT_READ_FAILURE, + /// The phase inductance estimator failed + PHASE_INDUCTANCE_ESTIMATOR_FAILURE, + /// The phase resistance estimator failed + PHASE_RESISTANCE_ESTIMATOR_FAILURE, /// The total number of errors TOTAL_ERROR_COUNT, }; @@ -150,6 +158,8 @@ class BrushlessFOCControlLoop : public ControlLoop { rotor_position_estimators_(rotor_position_estimators), num_rotor_position_estimators_(num_rotor_position_estimators), foc_controller_(clock), + phase_inductance_estimator_(clock, hwbridge::PhaseInductanceEstimatorController::Params()), + phase_resistance_estimator_(clock, hwbridge::PhaseResistanceEstimatorController::Params()), foc_frame_vars_() {} /** @@ -157,7 +167,7 @@ class BrushlessFOCControlLoop : public ControlLoop { * @param params The control loop parameters * @attention The parameters are not copied, so the parameters must remain in scope for the lifetime of the control loop */ - void init(BrushlessFOCControlLoopParams* params); + void init(Params* params); /** * @brief Run the control loop @@ -193,17 +203,19 @@ class BrushlessFOCControlLoop : public ControlLoop { protected: /*! \cond PRIVATE */ - BrushlessFOCControlLoopState state_ = BrushlessFOCControlLoopState::STOP; + State state_ = State::STOP; hwbridge::Bridge3Phase& bridge_; basilisk_hal::HAL_CLOCK& clock_; bldc_rotor_estimator::ElectricalRotorPosEstimator** rotor_position_estimators_ = nullptr; size_t num_rotor_position_estimators_ = 0; // Control loop parameters - BrushlessFOCControlLoopParams* params_ = nullptr; + Params* params_ = nullptr; BrushlessFOCControlLoopStatus status_; // Controllers FOCController foc_controller_; + hwbridge::PhaseInductanceEstimatorController phase_inductance_estimator_; + hwbridge::PhaseResistanceEstimatorController phase_resistance_estimator_; // Control loop state variables utime_t last_run_time_ = 0; @@ -217,10 +229,13 @@ class BrushlessFOCControlLoop : public ControlLoop { * @brief Get the desired state of the control loop * @param i_q_reference The desired quadrature current reference for the control loop to track * @param current_state The current state + * @param params The control loop parameters (used to determine if calibration is required) + * @param status The status of the control loop * @todo If required later, make this based on the current magnitude or perhaps another function to 'arm'? * @return The desired state of the control loop */ - BrushlessFOCControlLoopState get_desired_state(float i_q_reference, const BrushlessFOCControlLoopState current_state); + State get_desired_state(float i_q_reference, const State current_state, const Params& params, + const BrushlessFOCControlLoopStatus& status); /** * @brief update the rotor position estimator @@ -240,7 +255,7 @@ class BrushlessFOCControlLoop : public ControlLoop { */ void update_rotor_position_estimator(bldc_rotor_estimator::ElectricalRotorPosEstimator::EstimatorInputs& estimator_inputs, utime_t current_time_us, hwbridge::Bridge3Phase::phase_current_t phase_currents, - const BrushlessFOCControlLoopParams* params, math::alpha_beta_t V_alpha_beta, + const Params* params, math::alpha_beta_t V_alpha_beta, BrushlessFOCControlLoopStatus& status, float& theta, bldc_rotor_estimator::ElectricalRotorPosEstimator* rotor_position_estimators[], size_t num_rotor_position_estimators); @@ -251,7 +266,7 @@ class BrushlessFOCControlLoop : public ControlLoop { * @param desired_state The desired state to exit * @return void */ - void exit_state(const BrushlessFOCControlLoopState& current_state, const BrushlessFOCControlLoopState& desired_state); + void exit_state(const State& current_state, const State& desired_state); /** * @brief Enter a state @@ -259,7 +274,16 @@ class BrushlessFOCControlLoop : public ControlLoop { * @param desired_state The desired state to enter * @return void */ - void enter_state(const BrushlessFOCControlLoopState& current_state, const BrushlessFOCControlLoopState& desired_state); + void enter_state(const State& current_state, const State& desired_state); + + /** + * @brief Run the calibration routine + * @param FOC_inputs The FOC inputs (technically not fully required, but using it since it contains everything we'd possibly + * need) + * @param phase_commands The phase commands + * @return void + */ + void run_calibration(FOCController::FOCInputs inputs, hwbridge::Bridge3Phase::phase_command_t phase_commands[3]); /** * @brief Reset the position estimators @@ -288,8 +312,7 @@ class BrushlessFOCControlLoop : public ControlLoop { bldc_rotor_estimator::ElectricalRotorPosEstimator* rotor_position_estimators[], size_t num_rotor_position_estimators, hwbridge::Bridge3Phase& bridge, BrushlessFOCControlLoopStatus& status, math::alpha_beta_t V_alpha_beta, - const BrushlessFOCControlLoopParams* params, - math::direct_quad_t i_direct_quad_ref); + const Params* params, math::direct_quad_t i_direct_quad_ref); /*! \endcond */ }; diff --git a/control_loop/bldc/foc/foc_util.cpp b/control_loop/bldc/foc/foc_util.cpp index 8709e43..7d067f5 100644 --- a/control_loop/bldc/foc/foc_util.cpp +++ b/control_loop/bldc/foc/foc_util.cpp @@ -113,7 +113,7 @@ math::abc_t svpwm(math::alpha_beta_t V_alpha_beta, float Vbus) { // atan2f returns a value between -pi and pi, but we want a value between 0 and 2pi if (alpha < 0.0f) { - alpha += 2.0f * M_PI; + alpha += 2.0f * math::M_PI_FLOAT; } // Normalize our calculations to 1.0 time period, such that our switching times can be converted directly to duty cycles @@ -131,8 +131,8 @@ math::abc_t svpwm(math::alpha_beta_t V_alpha_beta, float Vbus) { // This is the term that is common to T1 and T2 for multiplication const float timing_multiplier_const = math::sqrt_3 * T_z * V_modulus / Vbus; - const float T1 = timing_multiplier_const * (sinf(sector / 3.0f * M_PI - alpha)); - const float T2 = timing_multiplier_const * (sinf(alpha - (sector_n_minus_1) / 3.0f * M_PI)); + const float T1 = timing_multiplier_const * (sinf(sector / 3.0f * math::M_PI_FLOAT - alpha)); + const float T2 = timing_multiplier_const * (sinf(alpha - (sector_n_minus_1) / 3.0f * math::M_PI_FLOAT)); const float T0 = T_z - T1 - T2; switch (sector) { @@ -183,27 +183,27 @@ uint8_t svm_sector(math::alpha_beta_t V_alpha_beta) { // Get the angle of the voltage vector const float theta = atan2f(V_alpha_beta.beta, V_alpha_beta.alpha); do { - if (theta >= 0.0f && theta < M_PI / 3.0f) { + if (theta >= 0.0f && theta < math::M_PI_FLOAT / 3.0f) { sector = 1; break; } - if (theta >= M_PI / 3.0f && theta < 2.0f * M_PI / 3.0f) { + if (theta >= math::M_PI_FLOAT / 3.0f && theta < 2.0f * math::M_PI_FLOAT / 3.0f) { sector = 2; break; } - if (theta >= 2.0f * M_PI / 3.0f && theta < M_PI) { + if (theta >= 2.0f * math::M_PI_FLOAT / 3.0f && theta < math::M_PI_FLOAT) { sector = 3; break; } - if (theta >= -M_PI && theta < -2.0f * M_PI / 3.0f) { + if (theta >= -math::M_PI_FLOAT && theta < -2.0f * math::M_PI_FLOAT / 3.0f) { sector = 4; break; } - if (theta >= -2.0f * M_PI / 3.0f && theta < -M_PI / 3.0f) { + if (theta >= -2.0f * math::M_PI_FLOAT / 3.0f && theta < -math::M_PI_FLOAT / 3.0f) { sector = 5; break; } - if (theta >= -M_PI / 3.0f && theta < 0.0f) { + if (theta >= -math::M_PI_FLOAT / 3.0f && theta < 0.0f) { sector = 6; break; } diff --git a/control_loop/bldc/rotor_estimator/rotor_estimator.cpp b/control_loop/bldc/rotor_estimator/rotor_estimator.cpp index 65adc5f..ec36cb1 100644 --- a/control_loop/bldc/rotor_estimator/rotor_estimator.cpp +++ b/control_loop/bldc/rotor_estimator/rotor_estimator.cpp @@ -316,8 +316,8 @@ app_hal_status_E SensorlessRotorFluxObserver::update(const EstimatorInputs& inpu } // Update the phase resistance and inductance values to use the equivalent Alpha-Beta parameters - const float stator_resistance = inputs.phase_resistance * 3.0 / 2.0; - const float stator_inductance = inputs.phase_inductance * 3.0 / 2.0; + const float stator_resistance = inputs.phase_resistance * 3.0f / 2.0f; + const float stator_inductance = inputs.phase_inductance * 3.0f / 2.0f; // Clarke transform the current math::alpha_beta_t i_ab = math::clarke_transform(inputs.phase_current.u, inputs.phase_current.v, inputs.phase_current.w); diff --git a/hal/hal_clock.hpp b/hal/hal_clock.hpp index d8f105d..758e0d6 100644 --- a/hal/hal_clock.hpp +++ b/hal/hal_clock.hpp @@ -24,7 +24,7 @@ class HAL_CLOCK { * @brief Get the current time in seconds * @return The current time in seconds */ - float get_time_s() { return get_time_us() / kMicrosecondsPerSecond; } + float get_time_s() { return static_cast(get_time_us()) / kMicrosecondsPerSecond; } /** * @brief Get the time difference between two times in microseconds diff --git a/hwbridge/3phase/bridge_3phase.hpp b/hwbridge/3phase/bridge_3phase.hpp index 231186d..341eb85 100644 --- a/hwbridge/3phase/bridge_3phase.hpp +++ b/hwbridge/3phase/bridge_3phase.hpp @@ -90,6 +90,14 @@ class Bridge3Phase { */ class phase_current_t { public: + phase_current_t() = default; + /** + * @brief Construct a phase current + * @param u Phase U current + * @param v Phase V current + * @param w Phase W current + */ + phase_current_t(float u, float v, float w) : u(u), v(v), w(w) {} /** * @brief Phase U current */ diff --git a/hwbridge/3phase/phase_inductance_estimator.cpp b/hwbridge/3phase/phase_inductance_estimator.cpp index ca8c795..edb196b 100644 --- a/hwbridge/3phase/phase_inductance_estimator.cpp +++ b/hwbridge/3phase/phase_inductance_estimator.cpp @@ -3,6 +3,14 @@ #include "math_util.hpp" namespace hwbridge { + +void PhaseInductanceEstimatorController::init(Params params) { + params_ = params; + state_ = State::NOT_STARTED; + brake_start_time_ = 0; + measurement_start_time_ = 0; +} + PhaseInductanceEstimatorController::Result PhaseInductanceEstimatorController::run_phase_inductance_estimator( PhaseInductanceEstimatorController::Input input) { Result result; diff --git a/hwbridge/3phase/phase_inductance_estimator.hpp b/hwbridge/3phase/phase_inductance_estimator.hpp index afe8d5b..2e06375 100644 --- a/hwbridge/3phase/phase_inductance_estimator.hpp +++ b/hwbridge/3phase/phase_inductance_estimator.hpp @@ -66,6 +66,12 @@ class PhaseInductanceEstimatorController { */ PhaseInductanceEstimatorController(basilisk_hal::HAL_CLOCK& clock, Params params) : params_(params), clock_(clock) {} + /** + * @brief Initialize (or reset) the Phase Inductance Estimator + * @param params Parameters for the Phase Inductance Estimator + */ + void init(Params params); + /** * @brief Run the Phase Inductance Estimator * @param input The input to the Phase Inductance Estimator diff --git a/hwbridge/3phase/phase_resistance_estimator.cpp b/hwbridge/3phase/phase_resistance_estimator.cpp index 9ad4889..2e7c502 100644 --- a/hwbridge/3phase/phase_resistance_estimator.cpp +++ b/hwbridge/3phase/phase_resistance_estimator.cpp @@ -5,6 +5,17 @@ #include "math_util.hpp" namespace hwbridge { +void PhaseResistanceEstimatorController::init(const Params& params) { + params_ = params; + state_ = State::NOT_STARTED; + brake_start_time_ = 0; + measurement_start_time_ = 0; + commanded_voltage_ = 0.0f; + current_controller_.set_kp(params_.current_kp); + current_controller_.set_ki(params_.current_ki); + current_controller_.set_kd(0.0f); + current_controller_.reset(); +} PhaseResistanceEstimatorController::Result PhaseResistanceEstimatorController::run_phase_resistance_estimator(Input input) { Result result; diff --git a/hwbridge/3phase/phase_resistance_estimator.hpp b/hwbridge/3phase/phase_resistance_estimator.hpp index 4c66a2d..88d1d77 100644 --- a/hwbridge/3phase/phase_resistance_estimator.hpp +++ b/hwbridge/3phase/phase_resistance_estimator.hpp @@ -87,6 +87,12 @@ class PhaseResistanceEstimatorController { clock_(clock), current_controller_(params_.current_kp, params_.current_ki, 0.0f, 0.0f, params_.max_voltage, 0.0f, clock_) {} + /** + * @brief Initialize (or reset) the Phase Resistance Estimator + * @param params Parameters for the Phase Resistance Estimator + */ + void init(const Params& params); + /** * @brief Run the Phase Resistance Estimator * @param input The inputs to the Phase Resistance Estimator diff --git a/test/brushless_control_loop_test.cpp b/test/brushless_control_loop_test.cpp index c65f256..19fe554 100644 --- a/test/brushless_control_loop_test.cpp +++ b/test/brushless_control_loop_test.cpp @@ -22,19 +22,33 @@ class BrushlessFOCControlLoopTest : public BrushlessFOCControlLoop { BrushlessFOCControlLoop::BrushlessFocControLoopParams foc_params_{ .current_control_bandwidth_rad_per_sec = 0.0f, - .phase_resistance = 0.0f, - .phase_inductance = 0.0f, - .pm_flux_linkage = 0.0f, + .phase_resistance = 1.0f, + .phase_resistance_valid = true, + + .phase_inductance = 1.0f, + .phase_inductance_valid = true, - .foc_start_timeout_period_us = 0, + .pm_flux_linkage = 0.0f, .disable_ki = false, .speed_to_iq_gain = 0.0f, .i_d_reference_default = 0.0f, - .pwm_control_type = BldcFoc::BrushlessFocPwmControlType::SPACE_VECTOR}; - - BrushlessFOCControlLoop::BrushlessFOCControlLoopParams test_params_{ - .foc_params = foc_params_, .open_loop_theta_velocity = 1.0f, .open_loop_quadrature_voltage = 1.0f}; + .pwm_control_type = BldcFoc::BrushlessFocPwmControlType::SPACE_VECTOR, + }; + + BrushlessFOCControlLoop::Params test_params_{ + .foc_params = foc_params_, + .open_loop_theta_velocity = 1.0f, + .open_loop_quadrature_voltage = 1.0f, + .phase_inductance_estimator_params = {.brake_duration = 1, .measurement_duration = 100}, + .phase_resistance_estimator_params = {.brake_duration = 1, + .measurement_duration = 10000, + .target_current = 1, + .current_tolerance = 0.1, + .max_voltage = 0.0f, + .current_kp = 1.0f, + .current_ki = 0.0f}, + }; BrushlessFOCControlLoopTest(bldc_rotor_estimator::ElectricalRotorPosEstimator& rotor_position_estimator, basilisk_hal::HAL_CLOCK& clock) @@ -58,13 +72,16 @@ TEST(BrushlessFOCControlLoopTest, test_stop_to_start_to_run) { // Call the desired state function with a time of 0, time at start of 0, and a motor speed of 0 // Ensure that the desired state is stop - EXPECT_EQ(test_control_loop.get_desired_state(0, BrushlessFOCControlLoop::BrushlessFOCControlLoopState::STOP), - BrushlessFOCControlLoop::BrushlessFOCControlLoopState::STOP); + BrushlessFOCControlLoop::BrushlessFOCControlLoopStatus status = BrushlessFOCControlLoop::BrushlessFOCControlLoopStatus(); + EXPECT_EQ( + test_control_loop.get_desired_state(0, BrushlessFOCControlLoop::State::STOP, test_control_loop.test_params_, status), + BrushlessFOCControlLoop::State::STOP); // Call the desired state function with a time of foc_start_timeout_period -1, time at start of 0, and a motor speed of 0.1, // with the rotor estimator valid Ensure that the desired state is run - EXPECT_EQ(test_control_loop.get_desired_state(0.1, BrushlessFOCControlLoop::BrushlessFOCControlLoopState::STOP), - BrushlessFOCControlLoop::BrushlessFOCControlLoopState::RUN); + EXPECT_EQ( + test_control_loop.get_desired_state(0.1, BrushlessFOCControlLoop::State::STOP, test_control_loop.test_params_, status), + BrushlessFOCControlLoop::State::RUN); } // Test the state machine transition from run to stop @@ -79,8 +96,9 @@ TEST(BrushlessFOCControlLoopTest, test_run_to_stop) { // Call the desired state function with a time of 0, time at start of 0, and a motor speed of 0 // Ensure that the desired state is stop - EXPECT_EQ(test_control_loop.get_desired_state(0, BrushlessFOCControlLoop::BrushlessFOCControlLoopState::RUN), - BrushlessFOCControlLoop::BrushlessFOCControlLoopState::STOP); + BrushlessFOCControlLoop::BrushlessFOCControlLoopStatus status = BrushlessFOCControlLoop::BrushlessFOCControlLoopStatus(); + EXPECT_EQ(test_control_loop.get_desired_state(0, BrushlessFOCControlLoop::State::RUN, test_control_loop.test_params_, status), + BrushlessFOCControlLoop::State::STOP); } // Test that a phase current read failure sets the error appropriately @@ -462,4 +480,105 @@ TEST(BrushlessFOCControlLoopTest, test_bus_voltage_read_failure) { EXPECT_TRUE(status.has_error(BrushlessFOCControlLoop::BrushlessFOCControlLoopError::BUS_VOLTAGE_READ_FAILURE)); } +// Test that the desired state is calibrating if either the phase resistance or phase inductance is indicated as invalid +TEST(BrushlessFOCControlLoopTest, test_desired_state_calibrating) { + // 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_; + test_params.foc_params.phase_inductance_valid = false; + + // Get the desired state + BrushlessFOCControlLoop::BrushlessFOCControlLoopStatus status = BrushlessFOCControlLoop::BrushlessFOCControlLoopStatus(); + BrushlessFOCControlLoop::State desired_state = + test_control_loop.get_desired_state(0.1, BrushlessFOCControlLoop::State::STOP, test_params, status); + + // Expect the desired state to be calibrating + EXPECT_EQ(desired_state, BrushlessFOCControlLoop::State::CALIBRATION); + + // Set the phase inductance to be valid, and the phase resistance to be invalid + test_params.foc_params.phase_inductance_valid = true; + test_params.foc_params.phase_resistance_valid = false; + + // Get the desired state + desired_state = test_control_loop.get_desired_state(0.1, BrushlessFOCControlLoop::State::STOP, test_params, status); + + // Expect the desired state to be calibrating + EXPECT_EQ(desired_state, BrushlessFOCControlLoop::State::CALIBRATION); +} + +// Test calibration -> run transition if both the phase resistance and phase inductance are valid +TEST(BrushlessFOCControlLoopTest, test_desired_state_calibration_to_run) { + // 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(); + BrushlessFOCControlLoop::State desired_state = + test_control_loop.get_desired_state(0.1, BrushlessFOCControlLoop::State::CALIBRATION, test_params, status); + + // Expect the desired state to be run + EXPECT_EQ(desired_state, BrushlessFOCControlLoop::State::RUN); +} + +// Test calibration to stop transition if both are valid but the iq reference is 0 +TEST(BrushlessFOCControlLoopTest, test_desired_state_calibration_to_stop_after_valid_calibration) { + // 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_; + + // Set the iq reference to 0 + test_params.open_loop_quadrature_voltage = 0.0f; + + // Get the desired state + BrushlessFOCControlLoop::BrushlessFOCControlLoopStatus status = BrushlessFOCControlLoop::BrushlessFOCControlLoopStatus(); + BrushlessFOCControlLoop::State desired_state = + test_control_loop.get_desired_state(0.0, BrushlessFOCControlLoop::State::CALIBRATION, test_params, status); + + // Expect the desired state to be run + EXPECT_EQ(desired_state, BrushlessFOCControlLoop::State::STOP); +} + +// Test that a failed phase estimation causes the desired state to go from calibration to stop +TEST(BrushlessFOCControlLoopTest, test_desired_state_calibration_to_stop_after_failed_calibration) { + // 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_; + + test_params.foc_params.phase_resistance_valid = true; + test_params.foc_params.phase_inductance_valid = false; + + // 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::CALIBRATION, test_params, status); + + EXPECT_EQ(desired_state, BrushlessFOCControlLoop::State::STOP); + + status.set_error(BrushlessFOCControlLoop::BrushlessFOCControlLoopError::PHASE_INDUCTANCE_ESTIMATOR_FAILURE, false); + status.set_error(BrushlessFOCControlLoop::BrushlessFOCControlLoopError::PHASE_RESISTANCE_ESTIMATOR_FAILURE, true); + + // Get the desired state + desired_state = test_control_loop.get_desired_state(0.1, BrushlessFOCControlLoop::State::CALIBRATION, test_params, status); + + EXPECT_EQ(desired_state, BrushlessFOCControlLoop::State::STOP); +} + } // namespace control_loop \ No newline at end of file diff --git a/util/math/math_util.hpp b/util/math/math_util.hpp index 2f35797..c77fa43 100644 --- a/util/math/math_util.hpp +++ b/util/math/math_util.hpp @@ -8,12 +8,12 @@ namespace math { -constexpr float sqrt_3 = 1.7320508075688772; -constexpr float sqrt_3_over_3 = 0.5773502691896258; +constexpr float sqrt_3 = 1.7320508075688772f; +constexpr float sqrt_3_over_3 = 0.5773502691896258f; constexpr float M_PI_FLOAT = static_cast(M_PI); -constexpr float ACCEPTABLE_FLOAT_ERROR = 0.000001; +constexpr float ACCEPTABLE_FLOAT_ERROR = 0.000001f; // Make a floated function to determine whether two floats are equal /** diff --git a/util/pid/pid.cpp b/util/pid/pid.cpp index 3804b72..836b3d7 100644 --- a/util/pid/pid.cpp +++ b/util/pid/pid.cpp @@ -47,14 +47,14 @@ T PID::calculate(T actual, T setpoint) { // Calculate the integral term integral += error * dt; // Clamp the integral term - if (integral_windup != 0) { + if (math::float_equals(integral_windup, 0) == false) { math::clamp(integral, -integral_windup, integral_windup); } float derivative = 0; // Calculate the derivative term - if (dt != 0) { + if (math::float_equals(dt, 0) == false) { derivative = (error - previous_error) / dt; } // Store the error for the next time @@ -64,7 +64,9 @@ T PID::calculate(T actual, T setpoint) { T output = error * kp + integral * ki + derivative * kd; // Clamp the output - if ((max_output != 0) || (min_output != 0)) { + const bool max_output_set = math::float_equals(max_output, 0) == false; + const bool min_output_set = math::float_equals(min_output, 0) == false; + if ((max_output_set) || (min_output_set)) { math::clamp(output, min_output, max_output); } return output;