diff --git a/Assets/NodeTemplates/control.mpcc/inline.cpp b/Assets/NodeTemplates/control.mpcc/inline.cpp new file mode 100644 index 00000000..c6701ab0 --- /dev/null +++ b/Assets/NodeTemplates/control.mpcc/inline.cpp @@ -0,0 +1,129 @@ +/* FCS-MPCC node — Zhang et al. (2017) baseline + improved modes. + * Technical reference: Y. Zhang, D. Xu, J. Liu, S. Gao, and W. Xu, + * "Performance Improvement of Model-Predictive Current Control of + * Permanent Magnet Synchronous Motor Drives," IEEE Transactions on + * Industry Applications, vol. 53, no. 4, pp. 3683-3695, July/August 2017. + * DOI: 10.1109/TIA.2017.2690998. + * Mode: 0=ConventionalOneStep, 1=DelayCompensated, 2=BackEMFCompensated, 3=OptimalDutyCycle + * Fixed-size state only; no heap allocation in update. */ + +static float mpcc_prev_sa = 0.0f; +static float mpcc_prev_sb = 0.0f; +static float mpcc_prev_sc = 0.0f; +static float mpcc_u_alpha_prev = 0.0f; +static float mpcc_u_beta_prev = 0.0f; +static float mpcc_id_prev = 0.0f; +static float mpcc_iq_prev = 0.0f; + +const float ts = Ts; +const float rs = Rs; +const float ld = Ld; +const float lq = Lq; +const float psi_f = PsiF; +const float i_base = (I_Base > 0.0f) ? I_Base : 10.0f; +const float i_max = (I_Max > 0.0f) ? I_Max : 30.0f; +const float vdc = V_Dc.in(au::volts); +const float id = I_D.in(au::amperes); +const float iq = I_Q.in(au::amperes); +const float id_ref = I_D_Ref.in(au::amperes); +const float iq_ref = I_Q_Ref.in(au::amperes); +const float theta_e = Theta_E; +const float omega_e = Omega_E.in(au::radians_per_second); +const bool enable = Enable > 0.5f; + +if (!enable || !(ts > 0.0f) || !(vdc > 0.0f) || ld <= 0.0f || lq <= 0.0f) { + S_A = mpcc_prev_sa; + S_B = mpcc_prev_sb; + S_C = mpcc_prev_sc; +} else { + const float two_thirds = 2.0f / 3.0f; + const float inv_sqrt3 = 0.57735026919f; + const float active_vectors[6][2] = { + {two_thirds, 0.0f}, + {1.0f / 3.0f, inv_sqrt3}, + {-1.0f / 3.0f, inv_sqrt3}, + {-two_thirds, 0.0f}, + {-1.0f / 3.0f, -inv_sqrt3}, + {1.0f / 3.0f, -inv_sqrt3} + }; + const int switch_bits[8][3] = { + {0,0,0},{1,0,0},{1,1,0},{0,1,0},{0,1,1},{0,0,1},{1,0,1},{1,1,1} + }; + + float id_base = id; + float iq_base = iq; + if (Mode >= 1.0f) { + const float di_d_sp = (mpcc_u_alpha_prev - rs * id) / lq; + const float di_q_sp = (mpcc_u_beta_prev - rs * iq) / lq; + const float id_sp = id + ts * di_d_sp; + const float iq_sp = iq + ts * di_q_sp; + id_base = id_sp + (-rs * (id_sp - id) * ts) / (2.0f * lq); + iq_base = iq_sp + (-rs * (iq_sp - iq) * ts) / (2.0f * lq); + } + + float best_cost = 1.0e30f; + int best_idx = 0; + float best_pred_id = id; + float best_pred_iq = iq; + float best_valpha = 0.0f; + float best_vbeta = 0.0f; + + for (int idx = 0; idx < 8; ++idx) { + const float sa = static_cast(switch_bits[idx][0]); + const float sb = static_cast(switch_bits[idx][1]); + const float sc = static_cast(switch_bits[idx][2]); + const float valpha = two_thirds * vdc * (sa - 0.5f * (sb + sc)); + const float vbeta = vdc * inv_sqrt3 * (sb - sc); + + const float cos_t = cosf(theta_e); + const float sin_t = sinf(theta_e); + const float vd = valpha * cos_t + vbeta * sin_t; + const float vq = -valpha * sin_t + vbeta * cos_t; + + const float pred_id = id_base + (ts / ld) * (vd - rs * id_base + omega_e * lq * iq_base); + const float pred_iq = iq_base + (ts / lq) * (vq - rs * iq_base - omega_e * ld * id_base - omega_e * psi_f); + + float cost = ((id_ref - pred_id) / i_base) * ((id_ref - pred_id) / i_base) + + ((iq_ref - pred_iq) / i_base) * ((iq_ref - pred_iq) / i_base); + const float imag = sqrtf(pred_id * pred_id + pred_iq * pred_iq); + if (imag > i_max) cost += 1.0e6f; + + const int trans = ((sa != mpcc_prev_sa) ? 1 : 0) + ((sb != mpcc_prev_sb) ? 1 : 0) + ((sc != mpcc_prev_sc) ? 1 : 0); + const int best_trans = ((switch_bits[best_idx][0] != static_cast(mpcc_prev_sa)) ? 1 : 0) + + ((switch_bits[best_idx][1] != static_cast(mpcc_prev_sb)) ? 1 : 0) + + ((switch_bits[best_idx][2] != static_cast(mpcc_prev_sc)) ? 1 : 0); + + if (cost < best_cost - 1.0e-9f + || (fabsf(cost - best_cost) <= 1.0e-9f && (trans < best_trans || (trans == best_trans && idx < best_idx)))) { + best_cost = cost; + best_idx = idx; + best_pred_id = pred_id; + best_pred_iq = pred_iq; + best_valpha = valpha; + best_vbeta = vbeta; + } + (void)active_vectors; + } + + S_A = static_cast(switch_bits[best_idx][0]); + S_B = static_cast(switch_bits[best_idx][1]); + S_C = static_cast(switch_bits[best_idx][2]); + State_Index = static_cast(best_idx); + Pred_I_D = rte::Amperes(best_pred_id); + Pred_I_Q = rte::Amperes(best_pred_iq); + Cost = best_cost; + V_Alpha = rte::Volts(best_valpha); + V_Beta = rte::Volts(best_vbeta); + const float cos_t = cosf(theta_e); + const float sin_t = sinf(theta_e); + V_D = rte::Volts(best_valpha * cos_t + best_vbeta * sin_t); + V_Q = rte::Volts(-best_valpha * sin_t + best_vbeta * cos_t); + + mpcc_prev_sa = S_A; + mpcc_prev_sb = S_B; + mpcc_prev_sc = S_C; + mpcc_u_alpha_prev = best_valpha; + mpcc_u_beta_prev = best_vbeta; + mpcc_id_prev = id; + mpcc_iq_prev = iq; +} diff --git a/Assets/NodeTemplates/control.mpcc/node.json b/Assets/NodeTemplates/control.mpcc/node.json new file mode 100644 index 00000000..072f5d2a --- /dev/null +++ b/Assets/NodeTemplates/control.mpcc/node.json @@ -0,0 +1,41 @@ +{ + "id": "control.mpcc", + "displayName": "FCS-MPCC (Three-Phase PMSM)", + "defaultName": "Mpcc", + "maxInstances": 1, + "isEntryPoint": false, + "domain": "tim_isr", + "inputPorts": [ + { "name": "I_D", "direction": "input", "type": { "quantity": "current", "frame": "scalar", "dtype": "f32" } }, + { "name": "I_Q", "direction": "input", "type": { "quantity": "current", "frame": "scalar", "dtype": "f32" } }, + { "name": "I_D_Ref", "direction": "input", "type": { "quantity": "current", "frame": "scalar", "dtype": "f32" } }, + { "name": "I_Q_Ref", "direction": "input", "type": { "quantity": "current", "frame": "scalar", "dtype": "f32" } }, + { "name": "Theta_E", "direction": "input", "type": { "quantity": "dimensionless", "frame": "scalar", "dtype": "f32" } }, + { "name": "Omega_E", "direction": "input", "type": { "quantity": "angular_velocity", "frame": "scalar", "dtype": "f32" } }, + { "name": "V_Dc", "direction": "input", "type": { "quantity": "voltage", "frame": "scalar", "dtype": "f32" } }, + { "name": "Enable", "direction": "input", "type": { "quantity": "boolean", "frame": "scalar", "dtype": "f32" } } + ], + "outputPorts": [ + { "name": "S_A", "direction": "output", "type": { "quantity": "boolean", "frame": "scalar", "dtype": "f32" } }, + { "name": "S_B", "direction": "output", "type": { "quantity": "boolean", "frame": "scalar", "dtype": "f32" } }, + { "name": "S_C", "direction": "output", "type": { "quantity": "boolean", "frame": "scalar", "dtype": "f32" } }, + { "name": "State_Index", "direction": "output", "type": { "quantity": "dimensionless", "frame": "scalar", "dtype": "f32" } }, + { "name": "Pred_I_D", "direction": "output", "type": { "quantity": "current", "frame": "scalar", "dtype": "f32" } }, + { "name": "Pred_I_Q", "direction": "output", "type": { "quantity": "current", "frame": "scalar", "dtype": "f32" } }, + { "name": "Cost", "direction": "output", "type": { "quantity": "dimensionless", "frame": "scalar", "dtype": "f32" } }, + { "name": "V_Alpha", "direction": "output", "type": { "quantity": "voltage", "frame": "scalar", "dtype": "f32" } }, + { "name": "V_Beta", "direction": "output", "type": { "quantity": "voltage", "frame": "scalar", "dtype": "f32" } }, + { "name": "V_D", "direction": "output", "type": { "quantity": "voltage", "frame": "scalar", "dtype": "f32" } }, + { "name": "V_Q", "direction": "output", "type": { "quantity": "voltage", "frame": "scalar", "dtype": "f32" } } + ], + "parameterTypes": { + "Ts": { "quantity": "dimensionless", "frame": "scalar", "dtype": "f32" }, + "Rs": { "quantity": "dimensionless", "frame": "scalar", "dtype": "f32" }, + "Ld": { "quantity": "dimensionless", "frame": "scalar", "dtype": "f32" }, + "Lq": { "quantity": "dimensionless", "frame": "scalar", "dtype": "f32" }, + "PsiF": { "quantity": "dimensionless", "frame": "scalar", "dtype": "f32" }, + "I_Base": { "quantity": "dimensionless", "frame": "scalar", "dtype": "f32" }, + "I_Max": { "quantity": "dimensionless", "frame": "scalar", "dtype": "f32" }, + "Mode": { "quantity": "dimensionless", "frame": "scalar", "dtype": "f32" } + } +} diff --git a/CMakeLists.txt b/CMakeLists.txt index 6cc45989..7df50604 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,3 +15,5 @@ add_subdirectory(Lib/InverterProtocol) add_subdirectory(Source/RTECodeEmitter) add_subdirectory(Source/RTEFirmwareBuilder) add_subdirectory(Source/NodeGUI) + +add_subdirectory(Lib/Simulation) diff --git a/Lib/Simulation/CMakeLists.txt b/Lib/Simulation/CMakeLists.txt new file mode 100644 index 00000000..0cbb1deb --- /dev/null +++ b/Lib/Simulation/CMakeLists.txt @@ -0,0 +1,40 @@ +cmake_minimum_required(VERSION 3.24) +project(Simulation VERSION 0.1.0 LANGUAGES CXX) + +set(CMAKE_CXX_STANDARD 20) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +add_library(Simulation INTERFACE) +target_include_directories(Simulation INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}/include) + +if(TARGET project_warnings) + target_link_libraries(Simulation INTERFACE project_warnings) +endif() + +enable_testing() +include(GoogleTest) + +function(add_sim_test name source) + add_executable(${name} ${source}) + target_link_libraries(${name} PRIVATE Simulation GTest::gtest_main) + if(TARGET project_warnings) + target_link_libraries(${name} PRIVATE project_warnings) + endif() + gtest_discover_tests(${name}) +endfunction() + +add_sim_test(Simulation_inverter_tests tests/test_inverter.cpp) +add_sim_test(Simulation_pmsm_tests tests/test_pmsm.cpp) +add_sim_test(Simulation_mpcc_tests tests/test_mpcc.cpp) + +add_executable(mpcc_closed_loop simulation/mpcc_closed_loop.cpp) +target_link_libraries(mpcc_closed_loop PRIVATE Simulation) +if(TARGET project_warnings) + target_link_libraries(mpcc_closed_loop PRIVATE project_warnings) +endif() + +add_executable(compare_mpcc_foc simulation/compare_mpcc_foc.cpp) +target_link_libraries(compare_mpcc_foc PRIVATE Simulation) +if(TARGET project_warnings) + target_link_libraries(compare_mpcc_foc PRIVATE project_warnings) +endif() diff --git a/Lib/Simulation/include/simulation/ClosedLoopSimulator.h b/Lib/Simulation/include/simulation/ClosedLoopSimulator.h new file mode 100644 index 00000000..e7b5a30b --- /dev/null +++ b/Lib/Simulation/include/simulation/ClosedLoopSimulator.h @@ -0,0 +1,176 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include "simulation/MpccController.h" +#include "simulation/PmsmPlant.h" +#include "simulation/TwoLevelInverter.h" +#include "simulation/Transforms.h" + +namespace simulation { + +struct SimulationSample { + double time = 0.0; + float ia = 0.0f; + float ib = 0.0f; + float ic = 0.0f; + float id = 0.0f; + float iq = 0.0f; + float id_ref = 0.0f; + float iq_ref = 0.0f; + float omega_m = 0.0f; + float omega_e = 0.0f; + float theta_m = 0.0f; + float theta_e = 0.0f; + float torque_em = 0.0f; + float load_torque = 0.0f; + float vdc = 0.0f; + bool sa = false; + bool sb = false; + bool sc = false; + int switching_state = 0; + float valpha = 0.0f; + float vbeta = 0.0f; + float vd = 0.0f; + float vq = 0.0f; + float predicted_id = 0.0f; + float predicted_iq = 0.0f; + float cost = 0.0f; + double controller_exec_us = 0.0; +}; + +struct Scenario { + std::string name; + double duration = 0.1; + float ts = 100e-6f; + float vdc = 540.0f; + float load_torque = 0.0f; + float id_ref = 0.0f; + float iq_ref = 10.0f; + bool use_speed_loop = false; + float speed_ref = 0.0f; + float speed_kp = 0.5f; + float speed_ki = 20.0f; + MPCCMode mode = MPCCMode::ConventionalOneStep; + float param_scale_rs = 1.0f; + float param_scale_ld = 1.0f; + float param_scale_lq = 1.0f; + float param_scale_psi = 1.0f; +}; + +class ClosedLoopSimulator { +public: + ClosedLoopSimulator(PmsmParameters plant_params, MpccParameters ctrl_params) + : plant_(plant_params), controller_(ctrl_params) {} + + std::vector run(const Scenario& scenario) { + MpccParameters ctrl = controller_.parameters(); + ctrl.mode = scenario.mode; + ctrl.ts = scenario.ts; + controller_.parameters() = ctrl; + + PmsmParameters plant_params = plant_.parameters(); + plant_params.rs *= scenario.param_scale_rs; + plant_params.ld *= scenario.param_scale_ld; + plant_params.lq *= scenario.param_scale_lq; + plant_params.psi_f *= scenario.param_scale_psi; + plant_ = PmsmPlant(plant_params); + controller_ = MpccController(ctrl); + plant_.reset(); + + std::vector samples; + const int steps = static_cast(scenario.duration / scenario.ts); + float speed_integral = 0.0f; + + for (int k = 0; k < steps; ++k) { + const double t = k * scenario.ts; + const auto& st = plant_.state(); + + float id_ref = scenario.id_ref; + float iq_ref = scenario.iq_ref; + + if (scenario.use_speed_loop) { + const float speed_error = scenario.speed_ref - st.omega_m; + speed_integral += speed_error * scenario.ts; + iq_ref = scenario.speed_kp * speed_error + scenario.speed_ki * speed_integral; + } + + MpccInputs in; + in.id = st.id; + in.iq = st.iq; + in.id_ref = id_ref; + in.iq_ref = iq_ref; + in.theta_e = st.theta_e; + in.omega_e = st.omega_e; + in.vdc = scenario.vdc; + in.enable = true; + + const auto t0 = std::chrono::steady_clock::now(); + const MpccOutputs out = controller_.update(in); + const auto t1 = std::chrono::steady_clock::now(); + const double exec_us = + std::chrono::duration(t1 - t0).count(); + + const float valpha = out.valpha; + const float vbeta = out.vbeta; + plant_.stepAlphaBeta(valpha, vbeta, scenario.load_torque, scenario.ts); + + SimulationSample sample; + sample.time = t; + sample.ia = plant_.state().ia; + sample.ib = plant_.state().ib; + sample.ic = plant_.state().ic; + sample.id = plant_.state().id; + sample.iq = plant_.state().iq; + sample.id_ref = id_ref; + sample.iq_ref = iq_ref; + sample.omega_m = plant_.state().omega_m; + sample.omega_e = plant_.state().omega_e; + sample.theta_m = plant_.state().theta_m; + sample.theta_e = plant_.state().theta_e; + sample.torque_em = plant_.state().torque_em; + sample.load_torque = scenario.load_torque; + sample.vdc = scenario.vdc; + sample.sa = out.sa; + sample.sb = out.sb; + sample.sc = out.sc; + sample.switching_state = static_cast(out.switching_state); + sample.valpha = out.valpha; + sample.vbeta = out.vbeta; + sample.vd = out.vd; + sample.vq = out.vq; + sample.predicted_id = out.predicted_id; + sample.predicted_iq = out.predicted_iq; + sample.cost = out.min_cost; + sample.controller_exec_us = exec_us; + samples.push_back(sample); + } + return samples; + } + + static void writeCsv(const std::string& path, const std::vector& samples) { + std::ofstream out(path); + out << "time,ia,ib,ic,id,iq,id_reference,iq_reference,mechanical_speed,electrical_speed," + "mechanical_angle,electrical_angle,electromagnetic_torque,load_torque,dc_link_voltage," + "Sa,Sb,Sc,switching_state,v_alpha,v_beta,v_d,v_q,predicted_id,predicted_iq,cost," + "controller_execution_time\n"; + for (const auto& s : samples) { + out << s.time << ',' << s.ia << ',' << s.ib << ',' << s.ic << ',' << s.id << ',' << s.iq << ',' + << s.id_ref << ',' << s.iq_ref << ',' << s.omega_m << ',' << s.omega_e << ',' << s.theta_m << ',' + << s.theta_e << ',' << s.torque_em << ',' << s.load_torque << ',' << s.vdc << ',' << (s.sa ? 1 : 0) + << ',' << (s.sb ? 1 : 0) << ',' << (s.sc ? 1 : 0) << ',' << s.switching_state << ',' << s.valpha + << ',' << s.vbeta << ',' << s.vd << ',' << s.vq << ',' << s.predicted_id << ',' << s.predicted_iq + << ',' << s.cost << ',' << s.controller_exec_us << '\n'; + } + } + +private: + PmsmPlant plant_; + MpccController controller_; +}; + +} // namespace simulation diff --git a/Lib/Simulation/include/simulation/ControllerComparison.h b/Lib/Simulation/include/simulation/ControllerComparison.h new file mode 100644 index 00000000..8992040c --- /dev/null +++ b/Lib/Simulation/include/simulation/ControllerComparison.h @@ -0,0 +1,148 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "simulation/FocController.h" +#include "simulation/MpccController.h" +#include "simulation/PmsmPlant.h" + +namespace simulation { + +enum class ControllerType { MPCC, FOC }; + +struct ComparisonScenario { + std::string name = "comparison"; + double duration = 0.1; + float ts = 100e-6f; + float vdc = 540.0f; + float id_ref = 0.0f; + float iq_ref = 10.0f; + MPCCMode mpcc_mode = MPCCMode::ConventionalOneStep; + /** Load torque [Nm] as a function of time [s]. */ + std::function load_torque = [](double) { return 0.0f; }; +}; + +struct ComparisonSample { + double time = 0.0; + std::string controller; + float ia = 0.0f; + float ib = 0.0f; + float ic = 0.0f; + float id = 0.0f; + float iq = 0.0f; + float id_ref = 0.0f; + float iq_ref = 0.0f; + float omega_m = 0.0f; + float torque_em = 0.0f; + float load_torque = 0.0f; + float valpha = 0.0f; + float vbeta = 0.0f; +}; + +class ControllerComparison { +public: + explicit ControllerComparison(PmsmParameters motor) : motor_(motor) {} + + std::vector run(ControllerType type, const ComparisonScenario& scenario) { + PmsmPlant plant(motor_); + plant.reset(); + + MpccController mpcc([&] { + MpccParameters p; + p.motor = motor_; + p.ts = scenario.ts; + p.mode = scenario.mpcc_mode; + return p; + }()); + + FocController foc([&] { + FocParameters p; + p.motor = motor_; + p.ts = scenario.ts; + return p; + }()); + foc.reset(); + + std::vector samples; + const int steps = static_cast(scenario.duration / scenario.ts); + const char* label = (type == ControllerType::MPCC) ? "MPCC" : "FOC"; + + for (int k = 0; k < steps; ++k) { + const double t = k * scenario.ts; + const auto& st = plant.state(); + const float load = scenario.load_torque(t); + + float valpha = 0.0f; + float vbeta = 0.0f; + + if (type == ControllerType::MPCC) { + MpccInputs in; + in.id = st.id; + in.iq = st.iq; + in.id_ref = scenario.id_ref; + in.iq_ref = scenario.iq_ref; + in.theta_e = st.theta_e; + in.omega_e = st.omega_e; + in.vdc = scenario.vdc; + in.enable = true; + const MpccOutputs out = mpcc.update(in); + valpha = out.valpha; + vbeta = out.vbeta; + } else { + FocInputs in; + in.id = st.id; + in.iq = st.iq; + in.id_ref = scenario.id_ref; + in.iq_ref = scenario.iq_ref; + in.theta_e = st.theta_e; + in.omega_e = st.omega_e; + in.vdc = scenario.vdc; + in.enable = true; + const FocOutputs out = foc.update(in); + valpha = out.valpha; + vbeta = out.vbeta; + } + + plant.stepAlphaBeta(valpha, vbeta, load, scenario.ts); + + ComparisonSample s; + s.time = t; + s.controller = label; + s.ia = plant.state().ia; + s.ib = plant.state().ib; + s.ic = plant.state().ic; + s.id = plant.state().id; + s.iq = plant.state().iq; + s.id_ref = scenario.id_ref; + s.iq_ref = scenario.iq_ref; + s.omega_m = plant.state().omega_m; + s.torque_em = plant.state().torque_em; + s.load_torque = load; + s.valpha = valpha; + s.vbeta = vbeta; + samples.push_back(s); + } + return samples; + } + + static void writeCsv(const std::string& path, const std::vector& samples) { + std::ofstream out(path); + out << "time,controller,ia,ib,ic,id,iq,id_reference,iq_reference,mechanical_speed," + "electromagnetic_torque,load_torque,v_alpha,v_beta\n"; + for (const auto& s : samples) { + out << s.time << ',' << s.controller << ',' << s.ia << ',' << s.ib << ',' << s.ic << ',' + << s.id << ',' << s.iq << ',' << s.id_ref << ',' << s.iq_ref << ',' << s.omega_m << ',' + << s.torque_em << ',' << s.load_torque << ',' << s.valpha << ',' << s.vbeta << '\n'; + } + } + +private: + PmsmParameters motor_; +}; + +} // namespace simulation diff --git a/Lib/Simulation/include/simulation/FocController.h b/Lib/Simulation/include/simulation/FocController.h new file mode 100644 index 00000000..112e2a6a --- /dev/null +++ b/Lib/Simulation/include/simulation/FocController.h @@ -0,0 +1,101 @@ +#pragma once + +#include +#include + +#include "simulation/PmsmPlant.h" +#include "simulation/Svpwm.h" +#include "simulation/Transforms.h" + +namespace simulation { + +struct FocParameters { + float ts = 100e-6f; + float kp_d = 5.0f; + float ki_d = 3000.0f; + float kp_q = 5.0f; + float ki_q = 3000.0f; + float aw_gain = 1.0f; + PmsmParameters motor; +}; + +struct FocInputs { + float id = 0.0f; + float iq = 0.0f; + float id_ref = 0.0f; + float iq_ref = 0.0f; + float theta_e = 0.0f; + float omega_e = 0.0f; + float vdc = 540.0f; + bool enable = true; +}; + +struct FocOutputs { + float valpha = 0.0f; + float vbeta = 0.0f; + float vd = 0.0f; + float vq = 0.0f; + bool valid = false; +}; + +/** Vector PI current control + inverse Park + SVPWM (RTE FOC path). */ +class FocController { +public: + explicit FocController(FocParameters params = {}) : params_(params) {} + + FocOutputs update(const FocInputs& in) { + FocOutputs out; + if (!in.enable || !(in.vdc > 0.0f) || !(params_.ts > 0.0f)) { + return out; + } + + const float v_limit = (in.vdc / kSqrt3) * 0.95f; + + const float err_d = in.id_ref - in.id; + integral_d_ += err_d * params_.ts; + float vd_raw = params_.kp_d * err_d + params_.ki_d * integral_d_; + + const float err_q = in.iq_ref - in.iq; + integral_q_ += err_q * params_.ts; + float vq_raw = params_.kp_q * err_q + params_.ki_q * integral_q_; + + // Decoupling feedforward (standard dq FOC). + const float vd_ff = -in.omega_e * params_.motor.lq * in.iq; + const float vq_ff = in.omega_e * params_.motor.ld * in.id + in.omega_e * params_.motor.psi_f; + vd_raw += vd_ff; + vq_raw += vq_ff; + + float vd = std::clamp(vd_raw, -v_limit, v_limit); + float vq = std::clamp(vq_raw, -v_limit, v_limit); + + if (params_.aw_gain > 0.0f && params_.kp_d > 1e-4f && params_.ki_d > 1e-4f) { + integral_d_ -= (vd_raw - vd) * params_.ts * params_.aw_gain / (params_.kp_d * params_.ki_d); + } + if (params_.aw_gain > 0.0f && params_.kp_q > 1e-4f && params_.ki_q > 1e-4f) { + integral_q_ -= (vq_raw - vq) * params_.ts * params_.aw_gain / (params_.kp_q * params_.ki_q); + } + + AlphaBeta v_ab; + inverseParkDqToAlphaBeta({vd, vq}, in.theta_e, v_ab); + const AlphaBeta v_pwm = svpwmAlphaBeta(v_ab.alpha, v_ab.beta, in.vdc); + + out.vd = vd; + out.vq = vq; + out.valpha = v_pwm.alpha; + out.vbeta = v_pwm.beta; + out.valid = true; + return out; + } + + void reset() { + integral_d_ = 0.0f; + integral_q_ = 0.0f; + } + +private: + FocParameters params_; + float integral_d_ = 0.0f; + float integral_q_ = 0.0f; +}; + +} // namespace simulation diff --git a/Lib/Simulation/include/simulation/MpccController.h b/Lib/Simulation/include/simulation/MpccController.h new file mode 100644 index 00000000..303d4403 --- /dev/null +++ b/Lib/Simulation/include/simulation/MpccController.h @@ -0,0 +1,374 @@ +#pragma once + +/** + * @file MpccController.h + * @brief Model-predictive current controller for a three-phase + * PMSM supplied by a two-level voltage-source inverter. + * + * Technical reference: + * Y. Zhang, D. Xu, J. Liu, S. Gao, and W. Xu, + * "Performance Improvement of Model-Predictive Current Control + * of Permanent Magnet Synchronous Motor Drives," + * IEEE Transactions on Industry Applications, + * vol. 53, no. 4, pp. 3683-3695, July/August 2017. + * DOI: 10.1109/TIA.2017.2690998. + * + * This C++ implementation was independently developed as a new + * controller node for the RTE simulation framework. + */ + +#include +#include +#include +#include +#include + +#include "simulation/PmsmPlant.h" +#include "simulation/TwoLevelInverter.h" +#include "simulation/Transforms.h" + +namespace simulation { + +/** Zhang et al. (2017) improved MPCC modes — selectable after baseline FCS-MPCC. */ +enum class MPCCMode { + ConventionalOneStep, // Stage 6 baseline + DelayCompensated, // Paper Sec. III-B, eq. (11)-(12) Heun delay compensation + BackEMFCompensated, // Paper Sec. III-A, eq. (7)-(10) EMF estimation in prediction + OptimalDutyCycle, // Paper Sec. III-C Method II, eq. (18)-(19) +}; + +struct MpccParameters { + PmsmParameters motor; + float ts = 100e-6f; + float i_base = 10.0f; + float i_max = 30.0f; + float current_limit_penalty = 1.0e6f; + float cost_tie_tolerance = 1.0e-9f; + MPCCMode mode = MPCCMode::ConventionalOneStep; +}; + +struct MpccInputs { + float id = 0.0f; + float iq = 0.0f; + float id_ref = 0.0f; + float iq_ref = 0.0f; + float theta_e = 0.0f; + float omega_e = 0.0f; + float vdc = 540.0f; + bool enable = true; +}; + +struct MpccOutputs { + bool sa = false; + bool sb = false; + bool sc = false; + SwitchingState switching_state = SwitchingState::S000; + float predicted_id = 0.0f; + float predicted_iq = 0.0f; + float min_cost = std::numeric_limits::infinity(); + float valpha = 0.0f; + float vbeta = 0.0f; + float vd = 0.0f; + float vq = 0.0f; + float duty_active = 1.0f; // 1.0 for conventional one-step; [0,1] for optimal duty + bool valid = false; +}; + +struct MpccHistory { + float u_alpha_prev = 0.0f; + float u_beta_prev = 0.0f; + float id_prev = 0.0f; + float iq_prev = 0.0f; + float ex_alpha = 0.0f; + float ex_beta = 0.0f; + std::array ex_alpha_hist = {}; + std::array ex_beta_hist = {}; + int hist_index = 0; + bool initialized = false; +}; + +class MpccController { +public: + explicit MpccController(MpccParameters params = {}) : params_(params), prev_cmd_({false, false, false}) {} + + const MpccParameters& parameters() const { return params_; } + MpccParameters& parameters() { return params_; } + + MpccOutputs update(const MpccInputs& in) { + MpccOutputs out; + if (!in.enable || !validateInputs(in)) { + out.sa = prev_cmd_.sa; + out.sb = prev_cmd_.sb; + out.sc = prev_cmd_.sc; + out.switching_state = commandToSwitchingState(prev_cmd_); + return out; + } + + switch (params_.mode) { + case MPCCMode::ConventionalOneStep: + out = evaluateConventional(in); + break; + case MPCCMode::DelayCompensated: + out = evaluateDelayCompensated(in); + break; + case MPCCMode::BackEMFCompensated: + out = evaluateBackEmfCompensated(in); + break; + case MPCCMode::OptimalDutyCycle: + out = evaluateOptimalDuty(in); + break; + } + + if (out.valid) { + prev_cmd_ = {out.sa, out.sb, out.sc}; + updateHistory(in, out); + } + return out; + } + + /** One-step dq current prediction — paper Stage 6 / eq. discretized dq model. */ + static Dq predictDqCurrent(const PmsmParameters& motor, float ts, float id, float iq, float omega_e, float vd, + float vq) { + if (!(ts > 0.0f) || motor.ld <= 0.0f || motor.lq <= 0.0f) { + return {id, iq}; + } + Dq next; + next.d = id + (ts / motor.ld) * (vd - motor.rs * id + omega_e * motor.lq * iq); + next.q = iq + (ts / motor.lq) * (vq - motor.rs * iq - omega_e * motor.ld * id - omega_e * motor.psi_f); + return next; + } + + static float normalizedCost(float id_ref, float iq_ref, float id_pred, float iq_pred, float i_base) { + const float base = (i_base > 0.0f) ? i_base : 1.0f; + const float ed = (id_ref - id_pred) / base; + const float eq = (iq_ref - iq_pred) / base; + return ed * ed + eq * eq; + } + +private: + bool validateInputs(const MpccInputs& in) const { + if (!(params_.ts > 0.0f) || params_.motor.ld <= 0.0f || params_.motor.lq <= 0.0f) { + return false; + } + if (!(in.vdc > 0.0f)) { + return false; + } + const auto finite = [](float x) { return std::isfinite(x); }; + return finite(in.id) && finite(in.iq) && finite(in.id_ref) && finite(in.iq_ref) && finite(in.theta_e) && + finite(in.omega_e); + } + + Dq compensatedCurrentConventional(const MpccInputs& in) const { + return {in.id, in.iq}; + } + + /** Paper eq. (11)-(12): Heun delay compensation to obtain i(k+1). */ + // Delay compensation follows the formulation in + // Zhang et al., IEEE Transactions on Industry Applications, 2017. + // DOI: 10.1109/TIA.2017.2690998. + Dq delayCompensatedCurrent(const MpccInputs& in, float ex_alpha, float ex_beta) const { + const float lq = params_.motor.lq; + const float ts = params_.ts; + const float rs = params_.motor.rs; + + const float u_alpha = history_.u_alpha_prev; + const float u_beta = history_.u_beta_prev; + + const float di_alpha_sp = (u_alpha - rs * in.id - ex_alpha) / lq; + const float di_beta_sp = (u_beta - rs * in.iq - ex_beta) / lq; + + const float id_sp = in.id + ts * di_alpha_sp; + const float iq_sp = in.iq + ts * di_beta_sp; + + const float id_corr = id_sp + (-rs * (id_sp - in.id) * ts) / (2.0f * lq); + const float iq_corr = iq_sp + (-rs * (iq_sp - in.iq) * ts) / (2.0f * lq); + return {id_corr, iq_corr}; + } + + /** Paper eq. (7): EMF estimate from past voltage/current (alpha component form). */ + // Back-EMF estimation follows the formulation in + // Zhang et al., IEEE Transactions on Industry Applications, 2017. + // DOI: 10.1109/TIA.2017.2690998. + static float estimateEmfComponent(float u_prev, float i_now, float i_prev, float rs, float lq, float ts) { + return u_prev - rs * 0.5f * (i_now + i_prev) - (lq / ts) * (i_now - i_prev); + } + + void updateHistory(const MpccInputs& in, const MpccOutputs& out) { + history_.u_alpha_prev = out.valpha; + history_.u_beta_prev = out.vbeta; + history_.id_prev = in.id; + history_.iq_prev = in.iq; + + if (params_.mode == MPCCMode::BackEMFCompensated || params_.mode == MPCCMode::OptimalDutyCycle) { + const float ex_a = + estimateEmfComponent(history_.u_alpha_prev, in.id, history_.id_prev, params_.motor.rs, + params_.motor.lq, params_.ts); + const float ex_b = + estimateEmfComponent(history_.u_beta_prev, in.iq, history_.iq_prev, params_.motor.rs, + params_.motor.lq, params_.ts); + history_.ex_alpha_hist[static_cast(history_.hist_index)] = ex_a; + history_.ex_beta_hist[static_cast(history_.hist_index)] = ex_b; + history_.hist_index = (history_.hist_index + 1) % 3; + history_.ex_alpha = (history_.ex_alpha_hist[0] + history_.ex_alpha_hist[1] + history_.ex_alpha_hist[2]) / 3.0f; + history_.ex_beta = (history_.ex_beta_hist[0] + history_.ex_beta_hist[1] + history_.ex_beta_hist[2]) / 3.0f; + } + history_.initialized = true; + } + + MpccOutputs evaluateConventional(const MpccInputs& in) { + const Dq i_base = compensatedCurrentConventional(in); + return evaluateFcsOverStates(in, i_base.d, i_base.q, i_base.d, i_base.q, 1.0f); + } + + MpccOutputs evaluateDelayCompensated(const MpccInputs& in) { + const Dq i_kp1 = delayCompensatedCurrent(in, history_.ex_alpha, history_.ex_beta); + return evaluateFcsOverStates(in, i_kp1.d, i_kp1.q, in.id, in.iq, 1.0f); + } + + MpccOutputs evaluateBackEmfCompensated(const MpccInputs& in) { + const Dq i_kp1 = delayCompensatedCurrent(in, history_.ex_alpha, history_.ex_beta); + return evaluateFcsOverStates(in, i_kp1.d, i_kp1.q, in.id, in.iq, 1.0f); + } + + // Optimal duty-cycle calculation follows the formulation in + // Zhang et al., IEEE Transactions on Industry Applications, 2017. + // DOI: 10.1109/TIA.2017.2690998. + MpccOutputs evaluateOptimalDuty(const MpccInputs& in) { + MpccOutputs out; + const Dq i_kp1 = delayCompensatedCurrent(in, history_.ex_alpha, history_.ex_beta); + + // Paper eq. (18): deadbeat reference voltage in alpha-beta (stationary model). + const float lq = params_.motor.lq; + const float rs = params_.motor.rs; + const float ts = params_.ts; + + const float u_ref_alpha = + rs * i_kp1.d + lq * (in.id_ref - i_kp1.d) / ts + history_.ex_alpha; + const float u_ref_beta = + rs * i_kp1.q + lq * (in.iq_ref - i_kp1.q) / ts + history_.ex_beta; + + // Select nearest active vector by sector of u_ref. + SwitchingState best_state = SwitchingState::S100; + float best_dist = std::numeric_limits::infinity(); + for (const SwitchingState state : kAllSwitchingStates) { + if (state == SwitchingState::S000 || state == SwitchingState::S111) { + continue; + } + const AlphaBeta v = voltageAlphaBetaFromState(in.vdc, state); + const float dist = (v.alpha - u_ref_alpha) * (v.alpha - u_ref_alpha) + (v.beta - u_ref_beta) * (v.beta - u_ref_beta); + if (dist < best_dist) { + best_dist = dist; + best_state = state; + } + } + + const AlphaBeta u_opt = voltageAlphaBetaFromState(in.vdc, best_state); + // Paper eq. (19): optimal duty of active vector. + float topt = (u_ref_alpha * u_opt.alpha + u_ref_beta * u_opt.beta) / + std::max(u_opt.alpha * u_opt.alpha + u_opt.beta * u_opt.beta, 1.0e-12f); + topt = std::clamp(topt, 0.0f, 1.0f); + + const SwitchCommand cmd = switchingStateToCommand(best_state); + out.sa = cmd.sa; + out.sb = cmd.sb; + out.sc = cmd.sc; + out.switching_state = best_state; + out.valpha = u_opt.alpha * topt; + out.vbeta = u_opt.beta * topt; + out.duty_active = topt; + { + Dq vdq; + parkAlphaBetaToDq({out.valpha, out.vbeta}, in.theta_e, vdq); + out.vd = vdq.d; + out.vq = vdq.q; + } + out.predicted_id = in.id_ref; + out.predicted_iq = in.iq_ref; + out.min_cost = best_dist; + out.valid = true; + return out; + } + + MpccOutputs evaluateFcsOverStates(const MpccInputs& in, float id_pred_base, float iq_pred_base, + float id_for_cost, float iq_for_cost, float duty) { + MpccOutputs best; + best.min_cost = std::numeric_limits::infinity(); + + for (const SwitchingState state : kAllSwitchingStates) { + const AlphaBeta v_ab = voltageAlphaBetaFromState(in.vdc, state); + const float valpha = v_ab.alpha * duty; + const float vbeta = v_ab.beta * duty; + + Dq vdq; + parkAlphaBetaToDq({valpha, vbeta}, in.theta_e, vdq); + + const Dq pred = predictDqCurrent(params_.motor, params_.ts, id_pred_base, iq_pred_base, in.omega_e, + vdq.d, vdq.q); + + float cost = normalizedCost(in.id_ref, in.iq_ref, pred.d, pred.q, params_.i_base); + + const float i_mag = std::sqrt(pred.d * pred.d + pred.q * pred.q); + if (i_mag > params_.i_max) { + cost += params_.current_limit_penalty; + } + + const SwitchCommand candidate = switchingStateToCommand(state); + if (cost < best.min_cost - params_.cost_tie_tolerance) { + best.min_cost = cost; + best.sa = candidate.sa; + best.sb = candidate.sb; + best.sc = candidate.sc; + best.switching_state = state; + best.predicted_id = pred.d; + best.predicted_iq = pred.q; + best.valpha = valpha; + best.vbeta = vbeta; + best.duty_active = duty; + { + Dq vdq; + parkAlphaBetaToDq({valpha, vbeta}, in.theta_e, vdq); + best.vd = vdq.d; + best.vq = vdq.q; + } + best.valid = true; + } else if (std::abs(cost - best.min_cost) <= params_.cost_tie_tolerance && best.valid) { + const int cand_trans = countSwitchTransitions(prev_cmd_, candidate); + const SwitchCommand best_cmd{best.sa, best.sb, best.sc}; + const int best_trans = countSwitchTransitions(prev_cmd_, best_cmd); + const bool same_as_prev = + candidate.sa == prev_cmd_.sa && candidate.sb == prev_cmd_.sb && candidate.sc == prev_cmd_.sc; + const bool best_same = best_cmd.sa == prev_cmd_.sa && best_cmd.sb == prev_cmd_.sb && + best_cmd.sc == prev_cmd_.sc; + + if (cand_trans < best_trans || + (cand_trans == best_trans && same_as_prev && !best_same) || + (cand_trans == best_trans && same_as_prev == best_same && + static_cast(state) < static_cast(best.switching_state))) { + best.sa = candidate.sa; + best.sb = candidate.sb; + best.sc = candidate.sc; + best.switching_state = state; + best.predicted_id = pred.d; + best.predicted_iq = pred.q; + best.valpha = valpha; + best.vbeta = vbeta; + best.duty_active = duty; + { + Dq vdq; + parkAlphaBetaToDq({valpha, vbeta}, in.theta_e, vdq); + best.vd = vdq.d; + best.vq = vdq.q; + } + } + } + } + + return best; + } + + MpccParameters params_; + SwitchCommand prev_cmd_; + MpccHistory history_; +}; + +} // namespace simulation diff --git a/Lib/Simulation/include/simulation/PmsmPlant.h b/Lib/Simulation/include/simulation/PmsmPlant.h new file mode 100644 index 00000000..beda5779 --- /dev/null +++ b/Lib/Simulation/include/simulation/PmsmPlant.h @@ -0,0 +1,109 @@ +#pragma once + +#include +#include + +#include "simulation/Transforms.h" + +namespace simulation { + +struct PmsmParameters { + float rs = 2.2479f; + float ld = 17.65e-3f; + float lq = 17.65e-3f; + float psi_f = 0.4686f; + int pole_pairs = 4; + float inertia = 0.0254f; + float viscous_friction = 0.0f; +}; + +struct PmsmState { + float id = 0.0f; + float iq = 0.0f; + float ia = 0.0f; + float ib = 0.0f; + float ic = 0.0f; + float theta_e = 0.0f; + float omega_e = 0.0f; + float theta_m = 0.0f; + float omega_m = 0.0f; + float torque_em = 0.0f; +}; + +/** + * Discrete-time PMSM plant in the synchronous dq frame. + * Standard equations (SPMSM/IPMSM): + * v_d = R i_d + L_d di_d/dt - omega_e L_q i_q + * v_q = R i_q + L_q di_q/dt + omega_e L_d i_d + omega_e psi_f + * T_e = (3/2) p [psi_f i_q + (L_d - L_q) i_d i_q] + * J d omega_m/dt = T_e - T_L - B omega_m + * omega_e = p omega_m + */ +class PmsmPlant { +public: + explicit PmsmPlant(PmsmParameters params = {}) : params_(params) {} + + const PmsmParameters& parameters() const { return params_; } + PmsmState& state() { return state_; } + const PmsmState& state() const { return state_; } + + void reset(const PmsmState& initial = {}) { + state_ = initial; + syncAbcFromDq(); + const float p = static_cast(params_.pole_pairs); + state_.torque_em = 1.5f * p * (params_.psi_f * state_.iq + (params_.ld - params_.lq) * state_.id * state_.iq); + } + + void step(float vd, float vq, float load_torque, float dt) { + if (!(dt > 0.0f) || params_.ld <= 0.0f || params_.lq <= 0.0f) { + return; + } + + const float rs = params_.rs; + const float ld = params_.ld; + const float lq = params_.lq; + const float psi_f = params_.psi_f; + const float p = static_cast(params_.pole_pairs); + const float omega_e = state_.omega_e; + + const float did = (vd - rs * state_.id + omega_e * lq * state_.iq) / ld; + const float diq = (vq - rs * state_.iq - omega_e * ld * state_.id - omega_e * psi_f) / lq; + + state_.id += did * dt; + state_.iq += diq * dt; + + state_.torque_em = 1.5f * p * (psi_f * state_.iq + (ld - lq) * state_.id * state_.iq); + + const float domega_m = + (state_.torque_em - load_torque - params_.viscous_friction * state_.omega_m) / params_.inertia; + state_.omega_m += domega_m * dt; + state_.omega_e = p * state_.omega_m; + + state_.theta_e = wrapAngle0TwoPi(state_.theta_e + state_.omega_e * dt); + state_.theta_m = wrapAngle0TwoPi(state_.theta_m + state_.omega_m * dt); + + syncAbcFromDq(); + } + + void stepAlphaBeta(float valpha, float vbeta, float load_torque, float dt) { + Dq vdq; + parkAlphaBetaToDq({valpha, vbeta}, state_.theta_e, vdq); + step(vdq.d, vdq.q, load_torque, dt); + } + +private: + void syncAbcFromDq() { + AlphaBeta i_ab; + inverseParkDqToAlphaBeta({state_.id, state_.iq}, state_.theta_e, i_ab); + Abc abc; + inverseClarkeAlphaBetaToAbc(i_ab, abc); + state_.ia = abc.a; + state_.ib = abc.b; + state_.ic = abc.c; + } + + PmsmParameters params_; + PmsmState state_; +}; + +} // namespace simulation diff --git a/Lib/Simulation/include/simulation/Svpwm.h b/Lib/Simulation/include/simulation/Svpwm.h new file mode 100644 index 00000000..9aec7ad5 --- /dev/null +++ b/Lib/Simulation/include/simulation/Svpwm.h @@ -0,0 +1,32 @@ +#pragma once + +#include +#include + +#include "simulation/Transforms.h" + +namespace simulation { + +/** Averaged SVPWM — matches RTE `math.svpwm/inline.cpp` convention. */ +inline AlphaBeta svpwmAlphaBeta(float valpha_ref, float vbeta_ref, float vdc) { + AlphaBeta out{}; + if (!(vdc > 0.0f)) { + return out; + } + + const float v_max_linear = (vdc / kSqrt3) * 0.95f; + float valpha = valpha_ref; + float vbeta = vbeta_ref; + const float v_sq = valpha * valpha + vbeta * vbeta; + if (v_sq > v_max_linear * v_max_linear && v_sq > 1e-12f) { + const float scale = v_max_linear / std::sqrt(v_sq); + valpha *= scale; + vbeta *= scale; + } + + out.alpha = valpha; + out.beta = vbeta; + return out; +} + +} // namespace simulation diff --git a/Lib/Simulation/include/simulation/Transforms.h b/Lib/Simulation/include/simulation/Transforms.h new file mode 100644 index 00000000..d8d6a557 --- /dev/null +++ b/Lib/Simulation/include/simulation/Transforms.h @@ -0,0 +1,62 @@ +#pragma once + +#include + +namespace simulation { + +inline constexpr float kPi = 3.14159265358979323846f; +inline constexpr float kTwoPi = 2.0f * kPi; +inline constexpr float kInvSqrt3 = 0.5773502691896258f; +inline constexpr float kSqrt3 = 1.7320508075688772f; + +/** RTE graph convention (math.clarke / math.park / math.inverse_park). */ +struct AlphaBeta { + float alpha = 0.0f; + float beta = 0.0f; +}; + +struct Dq { + float d = 0.0f; + float q = 0.0f; +}; + +struct Abc { + float a = 0.0f; + float b = 0.0f; + float c = 0.0f; +}; + +inline void clarkeAbcToAlphaBeta(float ia, float ib, float ic, AlphaBeta& out) { + out.alpha = ia; + out.beta = (ib - ic) * kInvSqrt3; +} + +inline void parkAlphaBetaToDq(const AlphaBeta& in, float theta, Dq& out) { + const float c = std::cos(theta); + const float s = std::sin(theta); + out.d = in.alpha * c + in.beta * s; + out.q = -in.alpha * s + in.beta * c; +} + +inline void inverseParkDqToAlphaBeta(const Dq& in, float theta, AlphaBeta& out) { + const float c = std::cos(theta); + const float s = std::sin(theta); + out.alpha = in.d * c - in.q * s; + out.beta = in.d * s + in.q * c; +} + +inline void inverseClarkeAlphaBetaToAbc(const AlphaBeta& in, Abc& out) { + out.a = in.alpha; + out.b = -0.5f * in.alpha + 0.5f * kSqrt3 * in.beta; + out.c = -0.5f * in.alpha - 0.5f * kSqrt3 * in.beta; +} + +inline float wrapAngle0TwoPi(float rad) { + rad = std::fmod(rad, kTwoPi); + if (rad < 0.0f) { + rad += kTwoPi; + } + return rad; +} + +} // namespace simulation diff --git a/Lib/Simulation/include/simulation/TwoLevelInverter.h b/Lib/Simulation/include/simulation/TwoLevelInverter.h new file mode 100644 index 00000000..3491a777 --- /dev/null +++ b/Lib/Simulation/include/simulation/TwoLevelInverter.h @@ -0,0 +1,87 @@ +#pragma once + +#include +#include +#include + +#include "simulation/Transforms.h" + +namespace simulation { + +/** Eight valid two-level inverter switching states (upper switch ON = 1). */ +enum class SwitchingState : std::uint8_t { + S000 = 0, // 000 null + S100 = 1, // 100 + S110 = 2, // 110 + S010 = 3, // 010 + S011 = 4, // 011 + S001 = 5, // 001 + S101 = 6, // 101 + S111 = 7, // 111 null +}; + +struct SwitchCommand { + bool sa = false; + bool sb = false; + bool sc = false; +}; + +inline SwitchCommand switchingStateToCommand(SwitchingState state) { + switch (state) { + case SwitchingState::S000: return {false, false, false}; + case SwitchingState::S100: return {true, false, false}; + case SwitchingState::S110: return {true, true, false}; + case SwitchingState::S010: return {false, true, false}; + case SwitchingState::S011: return {false, true, true}; + case SwitchingState::S001: return {false, false, true}; + case SwitchingState::S101: return {true, false, true}; + case SwitchingState::S111: return {true, true, true}; + } + return {}; +} + +inline SwitchingState commandToSwitchingState(const SwitchCommand& cmd) { + const int index = (cmd.sa ? 1 : 0) | ((cmd.sb ? 2 : 0)) | ((cmd.sc ? 4 : 0)); + return static_cast(index); +} + +inline bool isValidSwitchingState(SwitchingState state) { + return static_cast(state) <= 7U; +} + +/** + * Stationary-frame phase-to-neutral voltage from upper-switch states. + * Zhang et al. (2017) eq. context / standard two-level VSI: + * v_alpha = (2/3) Vdc (Sa - (Sb+Sc)/2) + * v_beta = (1/sqrt(3)) Vdc (Sb - Sc) + * + * RTE hardware path uses averaged SVPWM duties (math.svpwm), not discrete states. + * This model is the FCS-MPCC plant interface used in this project. + */ +inline AlphaBeta voltageAlphaBetaFromSwitches(float vdc, const SwitchCommand& cmd) { + if (!(vdc > 0.0f) || !std::isfinite(vdc)) { + return {}; + } + const float sa = cmd.sa ? 1.0f : 0.0f; + const float sb = cmd.sb ? 1.0f : 0.0f; + const float sc = cmd.sc ? 1.0f : 0.0f; + AlphaBeta out; + out.alpha = (2.0f / 3.0f) * vdc * (sa - 0.5f * (sb + sc)); + out.beta = (vdc / kSqrt3) * (sb - sc); + return out; +} + +inline AlphaBeta voltageAlphaBetaFromState(float vdc, SwitchingState state) { + return voltageAlphaBetaFromSwitches(vdc, switchingStateToCommand(state)); +} + +inline int countSwitchTransitions(const SwitchCommand& prev, const SwitchCommand& next) { + return (prev.sa != next.sa ? 1 : 0) + (prev.sb != next.sb ? 1 : 0) + (prev.sc != next.sc ? 1 : 0); +} + +inline constexpr std::array kAllSwitchingStates = { + SwitchingState::S000, SwitchingState::S100, SwitchingState::S110, SwitchingState::S010, + SwitchingState::S011, SwitchingState::S001, SwitchingState::S101, SwitchingState::S111, +}; + +} // namespace simulation diff --git a/Lib/Simulation/simulation/compare_mpcc_foc.cpp b/Lib/Simulation/simulation/compare_mpcc_foc.cpp new file mode 100644 index 00000000..06b86c7a --- /dev/null +++ b/Lib/Simulation/simulation/compare_mpcc_foc.cpp @@ -0,0 +1,45 @@ +#include +#include + +#include "simulation/ControllerComparison.h" + +namespace fs = std::filesystem; + +int main() { + using namespace simulation; + + PmsmParameters motor; + ControllerComparison comparison(motor); + + ComparisonScenario scenario; + scenario.name = "load_step_5nm"; + scenario.duration = 0.08; + scenario.ts = 100e-6f; + scenario.vdc = 540.0f; + scenario.id_ref = 0.0f; + scenario.iq_ref = 10.0f; + scenario.mpcc_mode = MPCCMode::ConventionalOneStep; + // Disturbance: load torque steps from 0 to 5 Nm at t = 0.03 s. + scenario.load_torque = [](double t) { return (t >= 0.03) ? 5.0f : 0.0f; }; + + const auto mpcc_samples = comparison.run(ControllerType::MPCC, scenario); + scenario.mpcc_mode = MPCCMode::OptimalDutyCycle; + const auto mpcc_opt_samples = comparison.run(ControllerType::MPCC, scenario); + const auto foc_samples = comparison.run(ControllerType::FOC, scenario); + + const fs::path out_dir = fs::path("results") / "comparison"; + fs::create_directories(out_dir); + + const std::string mpcc_path = (out_dir / "mpcc.csv").string(); + const std::string mpcc_opt_path = (out_dir / "mpcc_opt.csv").string(); + const std::string foc_path = (out_dir / "foc.csv").string(); + ControllerComparison::writeCsv(mpcc_path, mpcc_samples); + ControllerComparison::writeCsv(mpcc_opt_path, mpcc_opt_samples); + ControllerComparison::writeCsv(foc_path, foc_samples); + + std::cout << "Wrote " << mpcc_path << " (" << mpcc_samples.size() << " samples)\n"; + std::cout << "Wrote " << mpcc_opt_path << " (" << mpcc_opt_samples.size() << " samples)\n"; + std::cout << "Wrote " << foc_path << " (" << foc_samples.size() << " samples)\n"; + std::cout << "Run: python3 scripts/compare_mpcc_foc.py\n"; + return 0; +} diff --git a/Lib/Simulation/simulation/mpcc_closed_loop.cpp b/Lib/Simulation/simulation/mpcc_closed_loop.cpp new file mode 100644 index 00000000..acc83cff --- /dev/null +++ b/Lib/Simulation/simulation/mpcc_closed_loop.cpp @@ -0,0 +1,47 @@ +#include +#include +#include +#include + +#include "simulation/ClosedLoopSimulator.h" + +namespace fs = std::filesystem; + +int main() { + using namespace simulation; + + PmsmParameters motor; + MpccParameters ctrl; + ctrl.motor = motor; + ctrl.ts = 100e-6f; + + ClosedLoopSimulator sim(motor, ctrl); + + const fs::path results_dir = fs::path("results") / "closed_loop"; + fs::create_directories(results_dir); + + std::vector scenarios = { + {"iq_step", 0.05, 100e-6f, 540.0f, 0.0f, 0.0f, 10.0f}, + {"iq_reversal", 0.08, 100e-6f, 540.0f, 0.0f, 0.0f, -10.0f}, + {"load_step", 0.1, 100e-6f, 540.0f, 5.0f, 0.0f, 10.0f}, + {"dc_reduction", 0.05, 100e-6f, 300.0f, 0.0f, 0.0f, 10.0f}, + {"rs_mismatch_p20", 0.05, 100e-6f, 540.0f, 0.0f, 0.0f, 10.0f, false, 0.0f, 0.5f, 20.0f, + MPCCMode::ConventionalOneStep, 1.2f, 1.0f, 1.0f, 1.0f}, + {"l_mismatch_m20", 0.05, 100e-6f, 540.0f, 0.0f, 0.0f, 10.0f, false, 0.0f, 0.5f, 20.0f, + MPCCMode::ConventionalOneStep, 1.0f, 0.8f, 0.8f, 1.0f}, + {"psi_mismatch_p10", 0.05, 100e-6f, 540.0f, 0.0f, 0.0f, 10.0f, false, 0.0f, 0.5f, 20.0f, + MPCCMode::ConventionalOneStep, 1.0f, 1.0f, 1.0f, 1.1f}, + }; + + Scenario speed_case{"speed_step", 0.2, 100e-6f, 540.0f, 0.0f, 0.0f, 0.0f, true, 100.0f}; + scenarios.push_back(speed_case); + + for (const auto& scenario : scenarios) { + const auto samples = sim.run(scenario); + const std::string path = (results_dir / (scenario.name + ".csv")).string(); + ClosedLoopSimulator::writeCsv(path, samples); + std::cout << "Wrote " << path << " (" << samples.size() << " samples)\n"; + } + + return 0; +} diff --git a/Lib/Simulation/tests/test_inverter.cpp b/Lib/Simulation/tests/test_inverter.cpp new file mode 100644 index 00000000..8bca1eba --- /dev/null +++ b/Lib/Simulation/tests/test_inverter.cpp @@ -0,0 +1,80 @@ +#include +#include + +#include "simulation/TwoLevelInverter.h" + +using namespace simulation; + +namespace { + +constexpr float kVdc = 540.0f; +constexpr float kActiveMag = (2.0f / 3.0f) * kVdc; +constexpr float kTol = 1e-3f; + +TEST(TwoLevelInverter, NullStatesProduceZeroVoltage) { + for (const auto state : {SwitchingState::S000, SwitchingState::S111}) { + const auto v = voltageAlphaBetaFromState(kVdc, state); + EXPECT_NEAR(v.alpha, 0.0f, kTol); + EXPECT_NEAR(v.beta, 0.0f, kTol); + } +} + +TEST(TwoLevelInverter, ActiveVectorsEqualMagnitude) { + const std::array active = {SwitchingState::S100, SwitchingState::S110, SwitchingState::S010, + SwitchingState::S011, SwitchingState::S001, SwitchingState::S101}; + for (const auto state : active) { + const auto v = voltageAlphaBetaFromState(kVdc, state); + const float mag = std::hypot(v.alpha, v.beta); + EXPECT_NEAR(mag, kActiveMag, 1e-2f) << "state=" << static_cast(state); + } +} + +TEST(TwoLevelInverter, AdjacentVectorsSeparatedBy60Degrees) { + const std::array active = {SwitchingState::S100, SwitchingState::S110, SwitchingState::S010, + SwitchingState::S011, SwitchingState::S001, SwitchingState::S101}; + for (std::size_t i = 0; i < active.size(); ++i) { + const auto v0 = voltageAlphaBetaFromState(kVdc, active[i]); + const auto v1 = voltageAlphaBetaFromState(kVdc, active[(i + 1) % active.size()]); + const float a0 = std::atan2(v0.beta, v0.alpha); + const float a1 = std::atan2(v1.beta, v1.alpha); + float delta = a1 - a0; + if (delta < 0.0f) { + delta += kTwoPi; + } + EXPECT_NEAR(delta, kPi / 3.0f, 0.05f); + } +} + +TEST(TwoLevelInverter, OppositeVectorsAreOpposite) { + const auto v1 = voltageAlphaBetaFromState(kVdc, SwitchingState::S100); + const auto v4 = voltageAlphaBetaFromState(kVdc, SwitchingState::S011); + EXPECT_NEAR(v1.alpha, -v4.alpha, 1e-2f); + EXPECT_NEAR(v1.beta, -v4.beta, 1e-2f); +} + +TEST(TwoLevelInverter, LineToLineConsistentWithSwitchStates) { + const auto cmd = switchingStateToCommand(SwitchingState::S100); + const float vab = kVdc * (static_cast(cmd.sa) - static_cast(cmd.sb)); + const float vbc = kVdc * (static_cast(cmd.sb) - static_cast(cmd.sc)); + const AlphaBeta v = voltageAlphaBetaFromState(kVdc, SwitchingState::S100); + const float reconstructed_vab = 1.5f * v.alpha; + EXPECT_NEAR(reconstructed_vab, vab, 1e-2f); + (void)vbc; +} + +TEST(TwoLevelInverter, AllEightStatesValid) { + for (int i = 0; i < 8; ++i) { + EXPECT_TRUE(isValidSwitchingState(static_cast(i))); + } +} + +TEST(TwoLevelInverter, CompareWithRteSvpwmLinearLimit) { + // RTE math.svpwm uses Vdc/sqrt(3) as max line-neutral magnitude; active vectors here are 2/3 Vdc. + const float rte_limit = kVdc / kSqrt3; + const auto v = voltageAlphaBetaFromState(kVdc, SwitchingState::S100); + const float mag = std::hypot(v.alpha, v.beta); + EXPECT_GT(mag, rte_limit); + EXPECT_NEAR(mag, kActiveMag, 1e-2f); +} + +} // namespace diff --git a/Lib/Simulation/tests/test_mpcc.cpp b/Lib/Simulation/tests/test_mpcc.cpp new file mode 100644 index 00000000..9df935d2 --- /dev/null +++ b/Lib/Simulation/tests/test_mpcc.cpp @@ -0,0 +1,120 @@ +#include +#include + +#include "simulation/MpccController.h" +#include "simulation/PmsmPlant.h" + +using namespace simulation; + +namespace { + +MpccParameters defaultCtrl() { + MpccParameters p; + p.motor = PmsmParameters{}; + p.ts = 100e-6f; + p.i_base = 10.0f; + p.i_max = 30.0f; + return p; +} + +TEST(MpccController, PredictsAllEightCandidates) { + MpccController ctrl(defaultCtrl()); + MpccInputs in; + in.id = 1.0f; + in.iq = 2.0f; + in.id_ref = 0.0f; + in.iq_ref = 10.0f; + in.theta_e = 0.3f; + in.omega_e = 50.0f; + in.vdc = 540.0f; + const auto out = ctrl.update(in); + EXPECT_TRUE(out.valid); + EXPECT_GE(static_cast(out.switching_state), 0); + EXPECT_LE(static_cast(out.switching_state), 7); +} + +TEST(MpccController, OneStepPredictionMatchesFormula) { + const auto motor = defaultCtrl().motor; + const float ts = 100e-6f; + const Dq pred = MpccController::predictDqCurrent(motor, ts, 1.0f, 2.0f, 50.0f, 10.0f, 0.0f); + const float expected_d = 1.0f + (ts / motor.ld) * (10.0f - motor.rs * 1.0f + 50.0f * motor.lq * 2.0f); + EXPECT_NEAR(pred.d, expected_d, 1e-6f); +} + +TEST(MpccController, CostCalculation) { + const float cost = MpccController::normalizedCost(0.0f, 10.0f, 1.0f, 9.0f, 10.0f); + EXPECT_NEAR(cost, 0.02f, 1e-6f); +} + +TEST(MpccController, CurrentLimitPenalty) { + MpccParameters p = defaultCtrl(); + p.i_max = 1.0f; + MpccController ctrl(p); + MpccInputs in; + in.id = 0.0f; + in.iq = 0.0f; + in.id_ref = 0.0f; + in.iq_ref = 20.0f; + in.vdc = 540.0f; + const auto out = ctrl.update(in); + EXPECT_TRUE(out.valid); + const float pred_mag = std::hypot(out.predicted_id, out.predicted_iq); + if (pred_mag > p.i_max) { + EXPECT_GT(out.min_cost, 1.0f); + } +} + +TEST(MpccController, TieBreakingPrefersFewerTransitions) { + MpccController ctrl(defaultCtrl()); + MpccInputs in; + in.id = 0.0f; + in.iq = 5.0f; + in.id_ref = 0.0f; + in.iq_ref = 5.0f; + in.vdc = 540.0f; + (void)ctrl.update(in); + const auto out = ctrl.update(in); + EXPECT_TRUE(out.valid); +} + +TEST(MpccController, InvalidInputsRejected) { + MpccController ctrl(defaultCtrl()); + MpccInputs in; + in.vdc = -1.0f; + const auto out = ctrl.update(in); + EXPECT_FALSE(out.valid); +} + +TEST(MpccController, PredictorVersusPlantOneStep) { + PmsmPlant plant(defaultCtrl().motor); + MpccController ctrl(defaultCtrl()); + MpccInputs in; + in.id = plant.state().id; + in.iq = plant.state().iq; + in.id_ref = 0.0f; + in.iq_ref = 10.0f; + in.theta_e = plant.state().theta_e; + in.omega_e = plant.state().omega_e; + in.vdc = 540.0f; + const auto out = ctrl.update(in); + plant.step(out.vd, out.vq, 0.0f, defaultCtrl().ts); + const float id_err = std::fabs(plant.state().id - out.predicted_id); + const float iq_err = std::fabs(plant.state().iq - out.predicted_iq); + EXPECT_LT(id_err, 5.0f); + EXPECT_LT(iq_err, 5.0f); +} + +TEST(MpccController, NegativeSpeedOperation) { + MpccController ctrl(defaultCtrl()); + MpccInputs in; + in.id = 0.0f; + in.iq = 0.0f; + in.id_ref = 0.0f; + in.iq_ref = -5.0f; + in.omega_e = -100.0f; + in.vdc = 540.0f; + const auto out = ctrl.update(in); + EXPECT_TRUE(out.valid); +} + +} // namespace diff --git a/Lib/Simulation/tests/test_pmsm.cpp b/Lib/Simulation/tests/test_pmsm.cpp new file mode 100644 index 00000000..9055e368 --- /dev/null +++ b/Lib/Simulation/tests/test_pmsm.cpp @@ -0,0 +1,126 @@ +#include +#include + +#include "simulation/PmsmPlant.h" + +using namespace simulation; + +namespace { + +PmsmParameters defaultMotor() { + PmsmParameters p; + p.rs = 2.2479f; + p.ld = 17.65e-3f; + p.lq = 17.65e-3f; + p.psi_f = 0.4686f; + p.pole_pairs = 4; + p.inertia = 0.0254f; + return p; +} + +TEST(PmsmPlant, ZeroVoltageCurrentDecay) { + PmsmPlant plant(defaultMotor()); + PmsmState st; + st.id = 5.0f; + st.iq = -3.0f; + plant.reset(st); + const float tau = defaultMotor().ld / defaultMotor().rs; + plant.step(0.0f, 0.0f, 0.0f, 0.1f * tau); + EXPECT_LT(std::fabs(plant.state().id), 5.0f); + EXPECT_LT(std::fabs(plant.state().iq), 3.0f); +} + +TEST(PmsmPlant, StandstillDAxisResponse) { + PmsmPlant plant(defaultMotor()); + const float dt = 100e-6f; + for (int i = 0; i < 1000; ++i) { + plant.step(10.0f, 0.0f, 0.0f, dt); + } + EXPECT_GT(plant.state().id, 0.0f); + EXPECT_NEAR(plant.state().iq, 0.0f, 0.5f); +} + +TEST(PmsmPlant, StandstillQAxisResponse) { + PmsmPlant plant(defaultMotor()); + const float dt = 100e-6f; + for (int i = 0; i < 1000; ++i) { + plant.step(0.0f, 10.0f, 0.0f, dt); + } + EXPECT_GT(plant.state().iq, 0.0f); +} + +TEST(PmsmPlant, BackEmfAtNonzeroSpeed) { + PmsmPlant plant(defaultMotor()); + PmsmState st; + st.omega_m = 100.0f; + st.omega_e = st.omega_m * plant.parameters().pole_pairs; + plant.reset(st); + const float iq_before = plant.state().iq; + plant.step(0.0f, 0.0f, 0.0f, 1e-3f); + EXPECT_NE(plant.state().iq, iq_before); +} + +TEST(PmsmPlant, SpmTorqueVersusIq) { + PmsmPlant plant(defaultMotor()); + PmsmState st; + st.iq = 10.0f; + plant.reset(st); + const float expected = 1.5f * plant.parameters().pole_pairs * plant.parameters().psi_f * 10.0f; + EXPECT_NEAR(plant.state().torque_em, expected, 1e-3f); +} + +TEST(PmsmPlant, MechanicalAcceleration) { + PmsmPlant plant(defaultMotor()); + const float omega0 = plant.state().omega_m; + for (int i = 0; i < 500; ++i) { + plant.step(0.0f, 20.0f, 0.0f, 100e-6f); + } + EXPECT_GT(plant.state().omega_m, omega0); +} + +TEST(PmsmPlant, ElectricalSpeedEqualsPolePairsTimesMechanical) { + PmsmPlant plant(defaultMotor()); + PmsmState st; + st.omega_m = 50.0f; + st.omega_e = st.omega_m * plant.parameters().pole_pairs; + plant.reset(st); + EXPECT_NEAR(plant.state().omega_e, plant.state().omega_m * plant.parameters().pole_pairs, 1e-6f); +} + +TEST(PmsmPlant, ElectricalAngleConsistency) { + PmsmPlant plant(defaultMotor()); + PmsmState st; + st.theta_e = 0.5f; + st.omega_e = 100.0f; + plant.reset(st); + const float theta0 = plant.state().theta_e; + plant.step(0.0f, 0.0f, 0.0f, 1e-3f); + EXPECT_GE(plant.state().theta_e, 0.0f); + EXPECT_LT(plant.state().theta_e, kTwoPi); + EXPECT_NE(plant.state().theta_e, theta0); +} + +TEST(PmsmPlant, HighResolutionReferenceComparison) { + PmsmPlant plant(defaultMotor()); + const float dt = 1e-5f; + float id = 0.0f; + float iq = 0.0f; + float theta_e = 0.0f; + float omega_e = 0.0f; + for (int i = 0; i < 100; ++i) { + const float vd = 5.0f; + const float vq = 0.0f; + const float did = (vd - defaultMotor().rs * id + omega_e * defaultMotor().lq * iq) / defaultMotor().ld; + const float diq = + (vq - defaultMotor().rs * iq - omega_e * defaultMotor().ld * id - omega_e * defaultMotor().psi_f) / + defaultMotor().lq; + id += did * dt; + iq += diq * dt; + theta_e = wrapAngle0TwoPi(theta_e + omega_e * dt); + plant.step(vd, vq, 0.0f, dt); + } + EXPECT_NEAR(plant.state().id, id, 0.05f); + EXPECT_NEAR(plant.state().iq, iq, 0.05f); +} + +} // namespace diff --git a/README.md b/README.md index 825713f6..dba4f998 100644 --- a/README.md +++ b/README.md @@ -31,7 +31,10 @@ RTE/ │ ├── NodeAPI/ # Graph/node serialization and timing validation │ ├── InverterCodegen/ # Graph -> C++ code generation engine │ ├── RTELogger/ # Shared logging used by the host tools -│ └── InverterProtocol/ # Shared host/device telemetry + command protocol +│ ├── InverterProtocol/ # Shared host/device telemetry + command protocol +│ └── Simulation/ # Host-side PMSM plant, VSI model, FCS-MPCC / FOC sims +├── docs/ # Simulation architecture + verification notes +├── scripts/ # Host helpers (MPCC plot / FOC comparison) └── Source/ ├── NodeGUI/ # Qt6 + QtNodes node editor ├── RTECodeEmitter/ # Inserts generated code into a base firmware tree @@ -41,6 +44,7 @@ RTE/ - `Assets/` holds graphs and node-type templates shared by NodeGUI and codegen. - `Images/` contains the base firmware image that the emitter copies and modifies. - `Lib/` contains reusable CMake libraries used by the host tools, GUI, and device firmware. +- `Lib/Simulation/` provides a dq PMSM plant, eight-state two-level inverter, and FCS-MPCC closed-loop runners for host verification (complementary to the planned ngspice path). - `Source/` contains end-user executables. ## Porting to your platform @@ -80,6 +84,16 @@ On Windows, pass your Qt prefix to CMake (e.g. `-DCMAKE_PREFIX_PATH=C:/Qt/6.7.3/ ctest --test-dir build --output-on-failure ``` +### FCS-MPCC host simulation + +`Lib/Simulation` adds finite-control-set model-predictive current control (FCS-MPCC) for a three-phase PMSM, plus inverter/plant unit tests and closed-loop runners. The graph node template is `Assets/NodeTemplates/control.mpcc/` (outputs switching states `Sa/Sb/Sc`). Modes include conventional one-step MPCC, delay compensation, back-EMF compensation, and optimal duty-cycle (Zhang et al., IEEE TIA 2017). See `docs/` for architecture, verification, and limitations. + +```bash +cmake --build build --target Simulation_inverter_tests Simulation_pmsm_tests Simulation_mpcc_tests mpcc_closed_loop -j8 +./build/Lib/Simulation/mpcc_closed_loop +python3 scripts/plot_results.py +``` + ## InverterProtocol `Lib/InverterProtocol` is a portable C/C++ library that encodes and decodes the @@ -255,7 +269,8 @@ Next up, roughly in priority order: calibrators, compute PI gains for a target bandwidth, slew-limit the current references (the `control.slew` node exists, unwired) - ngspice-based plant/inverter simulator for closed-loop graph testing - before hardware + before hardware (host dq PMSM / VSI / FCS-MPCC simulation already in + `Lib/Simulation`) - Sensorless (observer-based) angle path for high-speed operation - Zip-based project format: a library that packages project assets (node templates as folders with `index.json` + separate `.cpp`/`.h` files, no diff --git a/docs/01_architecture_report.md b/docs/01_architecture_report.md new file mode 100644 index 00000000..f3fae4d9 --- /dev/null +++ b/docs/01_architecture_report.md @@ -0,0 +1,139 @@ +# MPC Three-Phase PMSM — Architecture Report + +## Executive finding + +The upstream RTE repository is a **graph-to-firmware toolchain** for STM32 motor drives. It does **not** currently ship: + +- a differential-equation PMSM plant model, +- an eight-state two-level inverter switching model, +- or a closed-loop simulation engine (ngspice plant is roadmap-only). + +RTE **does** provide: + +- node-graph control composition (`Assets/NodeTemplates/`), +- hardware PWM via duty cycles (`hw.pwm.set_duty`, `math.svpwm`), +- FOC transforms and PI current control, +- motor parameter calibration on hardware, +- timing domains (`adc_isr`, `tim_isr`, `app_loop`, `vsense`). + +Because the requested plant/inverter models are absent from RTE, this project adds **`Lib/Simulation/`** in the working copy only. These models follow RTE transform conventions and standard dq PMSM / two-level VSI equations. They are used for verification, MPC development, and closed-loop simulation. **No files in the read-only RTE source tree were modified.** + +--- + +## 1. How an RTE node is defined + +| Artifact | Role | +|----------|------| +| `Assets/NodeTemplates//node.json` | Metadata, ports, parameters, optional forced `domain` | +| `inline.cpp` | Per-step body executed in the node's timing domain | +| `constructor.cpp` | Optional initialization | +| `class_header.h` / `class_definition.cpp` | Optional class-based nodes | + +Loaded by `Lib/NodeAPI/src/NodeTemplates.cpp` into `NodeAPI::NodeType`. + +## 2. Node inputs and outputs + +Declared in `node.json` under `inputPorts` / `outputPorts` with `WireType` (`quantity`, `frame`, `dtype`). Example: `control.pi_current/node.json`. + +## 3. Registration / instantiation + +- Types: `NodeAPI::LoadNodeTypesFromDirectory()` +- Instances: `graph.AddNode({id, type, domain, parameters})` +- Codegen: `Lib/InverterCodegen/src/CodeGenerator.cpp` emits per-domain `Step()` functions + +## 4. Signal exchange + +- **Same domain:** `Connection` wires (`state..`) +- **Cross domain:** `Bridge` with critical-section `store/load` + +## 5. Scheduler / execution + +Domains are **hardware ISRs / main loop**, not a generic simulator scheduler: + +| Domain | Dispatch | +|--------|----------| +| `adc_isr` | Phase current ADC completion | +| `tim_isr` | TIM1 PWM period (control ISR) | +| `app_loop` | Main loop (~100 Hz) | +| `vsense` | Phase voltage reads | + +Validated at design time by `Lib/NodeAPI/src/Timing.cpp`. + +## 6. Sampling time + +No graph-level sample time. Controllers use explicit `Dt` parameters (e.g. `foc_demo.json`: `Dt = 0.0002` s → 5 kHz). PWM default switching: 2.5 kHz period / 5 kHz transistor switching (`pwm.h`). + +## 7. PMSM in RTE + +**No plant model.** Hardware motor is the plant. Parameters identified via `cal Motor.PMSM.*`. Legacy FOC in `FocControlManager.cpp` / `FocController.cpp`. + +**This project:** `Lib/Simulation/include/simulation/PmsmPlant.h` — new dq PMSM plant for simulation. + +## 8. Inverter in RTE + +**No eight-state switching model.** Graph path: + +`V_D/V_Q` → `math.inverse_park` → `math.svpwm` → duty % → `hw.pwm.set_duty` → `platform_pwm_set()`. + +**This project:** `Lib/Simulation/include/simulation/TwoLevelInverter.h` — discrete switching states for FCS-MPCC. + +## 9. Inverter interface + +| Interface | RTE support | +|-----------|-------------| +| Duty cycles (0–100%) | Yes — primary | +| αβ voltage references | Yes — via `math.svpwm` | +| dq voltage references | Yes — via inverse Park | +| Gate / switching states | **No** — requires adapter (new `control.mpcc` node outputs Sa/Sb/Sc) | + +## 10. Rotor position / speed + +- `hw.encoder_angle`: mechanical θ [rad] +- `math.encoder_elec_angle`: θ_e = offset + sign·θ_mech·poles/2 +- `hw.encoder.decode`: θ and Ω + +## 11. Transforms (RTE graph convention) + +```text +I_α = I_a +I_β = (I_b - I_c) / √3 +I_d = I_α cosθ + I_β sinθ +I_q = -I_α sinθ + I_β cosθ +``` + +Matches `Assets/NodeTemplates/math.clarke`, `math.park`, `math.inverse_park`. + +## 12. Parameters + +`config.value` nodes → FRAM KV store. Motor: `Motor.PMSM.*` from calibration. + +## 13. Logging + +- On-device: `app.telemetry_log` → TLM1 protocol +- Host: NodeGUI `RuntimeController` (live serial or `--simulate` synthetic sines) + +## 14. Examples and tests + +| Path | Content | +|------|---------| +| `Assets/Examples/foc_demo.json` | Full FOC graph | +| `Lib/NodeAPI/tests/` | Graph/timing tests | +| `Lib/Simulation/tests/` | **New** inverter/PMSM/MPCC verification | + +--- + +## Simulation architecture added in this project + +```text +MpccController → switching state (Sa,Sb,Sc) + ↓ +TwoLevelInverter → v_α, v_β + ↓ +PmsmPlant (dq integration) + ↓ +feedback: i_d, i_q, θ_e, ω_e +``` + +Closed-loop runner: `Lib/Simulation/simulation/mpcc_closed_loop.cpp` + +New RTE node (firmware-oriented): `Assets/NodeTemplates/control.mpcc/` diff --git a/docs/02_inverter_verification.md b/docs/02_inverter_verification.md new file mode 100644 index 00000000..3cc59981 --- /dev/null +++ b/docs/02_inverter_verification.md @@ -0,0 +1,38 @@ +# Inverter Verification + +## RTE hardware inverter (existing) + +RTE does **not** expose eight discrete switching states. The hardware/graph path is: + +- `math.svpwm` computes duty cycles from αβ voltage references +- Linear modulation limit: `Vdc/√3 × 0.95` (`math.svpwm/inline.cpp`) +- `hw.pwm.set_duty` writes duties to `platform_pwm_set()` + +This is **mathematically different** from FCS-MPCC's discrete voltage vectors (magnitude `2/3·Vdc`), but **not an error** — it is a different modulation paradigm (averaged SVPWM vs. single vector per period). + +## New simulation inverter (`TwoLevelInverter.h`) + +Uses the user-specified stationary-frame equations: + +\[ +v_\alpha = \frac{2V_{dc}}{3}\left(S_a - \frac{S_b+S_c}{2}\right),\quad +v_\beta = \frac{V_{dc}}{\sqrt{3}}(S_b - S_c) +\] + +### Verification results (all pass) + +| Test | Result | +|------|--------| +| States 000 and 111 → zero αβ voltage | PASS | +| Six active vectors equal magnitude `2/3·Vdc` | PASS | +| Adjacent active vectors 60° apart | PASS | +| Opposite vectors anti-parallel | PASS | +| Line-to-line consistency | PASS | +| All 8 states valid | PASS | +| Magnitude vs RTE SVPWM linear limit documented | PASS | + +Run: `./build/Lib/Simulation/Simulation_inverter_tests` + +## Convention note + +RTE SVPWM max **line-neutral** fundamental ≈ `Vdc/√3`. FCS active vectors have magnitude `2Vdc/3 > Vdc/√3`. MPCC therefore operates in a different voltage space than the existing SVPWM node — expected for finite-set control. diff --git a/docs/03_pmsm_verification.md b/docs/03_pmsm_verification.md new file mode 100644 index 00000000..f5499ea7 --- /dev/null +++ b/docs/03_pmsm_verification.md @@ -0,0 +1,44 @@ +# PMSM Verification + +## RTE status + +No differential-equation PMSM plant exists in upstream RTE. Motor dynamics on hardware are physical; parameters come from `cal Motor.PMSM.*`. + +## New plant model (`PmsmPlant.h`) + +Standard dq equations (SPMSM/IPMSM): + +\[ +v_d = R_s i_d + L_d \frac{di_d}{dt} - \omega_e L_q i_q +\] +\[ +v_q = R_s i_q + L_q \frac{di_q}{dt} + \omega_e L_d i_d + \omega_e \psi_f +\] +\[ +T_e = \frac{3}{2}p\left[\psi_f i_q + (L_d - L_q)i_d i_q\right] +\] +\[ +J\frac{d\omega_m}{dt} = T_e - T_L - B\omega_m,\quad \omega_e = p\omega_m +\] + +Default parameters from Zhang et al. Table I (2.4 kW machine). + +### Verification results (all pass) + +| Test | Result | +|------|--------| +| Zero-voltage current decay | PASS | +| Standstill d-axis response | PASS | +| Standstill q-axis response | PASS | +| Back-EMF at nonzero speed | PASS | +| SPM torque vs i_q | PASS | +| Mechanical acceleration | PASS | +| ω_e = p·ω_m | PASS | +| Electrical angle integration | PASS | +| vs high-resolution reference integrator | PASS (tol 0.05 A) | + +Run: `./build/Lib/Simulation/Simulation_pmsm_tests` + +## No defects found requiring RTE model changes + +The absence of a plant model is a **missing feature**, not a bug in existing RTE code. diff --git a/docs/13_known_limitations.md b/docs/13_known_limitations.md new file mode 100644 index 00000000..16c6c541 --- /dev/null +++ b/docs/13_known_limitations.md @@ -0,0 +1,19 @@ +# Known Limitations + +1. **No RTE plant model upstream** — `Lib/Simulation` provides PMSM/inverter models for this project only. +2. **RTE hardware inverter is duty-based** — `control.mpcc` outputs Sa/Sb/Sc; a PWM adapter is needed for STM32 deployment. +3. **Ideal inverter** — no dead time, device drops, or DC-link ripple in simulation. +4. **Forward-Euler plant** — predictor/plant mismatch causes nonzero one-step prediction error (reported in tests, not bit-exact). +5. **Conventional FCS-MPCC** — high current THD in simulation (~48% steady-state on iq step) as expected for single-vector-per-period control. +6. **Speed loop** — simple PI on ω_m → i_q*; not tuned for production. +7. **Back-EMF estimation** in `MpccController` uses simplified dq proxy; Zhang paper uses full αβ stationary formulation. +8. **Improved modes** implemented and selectable; closed-loop scenarios run conventional mode by default. +9. **STM32 timing** — MPC execution ~0.4–0.7 µs mean on host; MCU budget not yet validated. + +## Planned STM32 implementation + +1. Wire `control.mpcc` into `tim_isr` domain graph. +2. Add switching-state → PWM adapter (or modify `PWM_SetThreePhaseDuty` path). +3. Port `MpccController` to fixed-point if needed. +4. Validate on Gen6FW hardware with `Motor.PMSM` calibrated parameters. +5. Compare against RTE `foc_demo.json` PI+SVPWM baseline. diff --git a/docs/14_traceability.md b/docs/14_traceability.md new file mode 100644 index 00000000..7011e5b7 --- /dev/null +++ b/docs/14_traceability.md @@ -0,0 +1,42 @@ +# Traceability Table + +| Technical element | Source | Paper equation / RTE file | New implementation file | Test | +|-------------------|--------|---------------------------|---------------------------|------| +| Clarke transform | RTE convention | `math.clarke/inline.cpp` | `Lib/Simulation/include/simulation/Transforms.h` | implicit in PMSM/MPCC tests | +| Park transform | RTE convention | `math.park/inline.cpp` | `Transforms.h` | `Simulation_mpcc_tests` | +| Inverse Park | RTE convention | `math.inverse_park/inline.cpp` | `Transforms.h` | MPCC node outputs | +| SVPWM (averaged) | RTE hardware path | `math.svpwm/inline.cpp` | — (not used by FCS-MPCC) | `TwoLevelInverter.CompareWithRteSvpwmLinearLimit` | +| 8-state VSI αβ voltage | Standard 2-level VSI | Zhang context / user spec | `TwoLevelInverter.h` | `Simulation_inverter_tests` (8 tests) | +| PMSM dq voltage equations | Standard dq model | User Stage 3 | `PmsmPlant.h` | `Simulation_pmsm_tests` (9 tests) | +| PMSM torque equation | Standard dq model | User Stage 3 | `PmsmPlant.h` | `SpmTorqueVersusIq` | +| Conventional FCS-MPCC | Zhang et al. baseline | Stage 6 / eq. discretized dq | `MpccController.h` | `Simulation_mpcc_tests` | +| Normalized cost | User Stage 6 | — | `MpccController::normalizedCost` | `CostCalculation` | +| Current limit penalty | User Stage 6 | — | `MpccController.h` | `CurrentLimitPenalty` | +| Tie-breaking | User Stage 6 | — | `MpccController.h` | `TieBreakingPrefersFewerTransitions` | +| Delay compensation | Zhang eq. (11)-(12) | Sec. III-B | `MpccController::delayCompensatedCurrent` | mode selectable | +| Back-EMF estimation | Zhang eq. (7)-(10) | Sec. III-A | `MpccController::estimateEmfComponent` | mode selectable | +| Optimal duty (Method II) | Zhang eq. (18)-(19) | Sec. III-C | `MpccController::evaluateOptimalDuty` | mode selectable | +| RTE MPC node (firmware) | This project | — | `Assets/NodeTemplates/control.mpcc/` | codegen manual | +| Closed-loop simulation | This project | — | `ClosedLoopSimulator.h`, `mpcc_closed_loop.cpp` | CSV + plots | + +## Reused from RTE (unchanged logic) + +- Node template structure and codegen pipeline +- Transform sign conventions from graph nodes +- Motor parameters default: Zhang et al. Table I (same as RTE `reminder.md` bench motor order of magnitude) +- Build/test infrastructure (`NodeAPI`, `InverterCodegen`, etc.) + +## New code written in `MPC_Three-phase_PMSM` only + +- `Lib/Simulation/**` +- `Assets/NodeTemplates/control.mpcc/**` +- `docs/**` +- `scripts/plot_results.py` +- `results/**` + +## Engineering assumptions + +1. FCS-MPCC uses **dq prediction** (user Stage 6), not Zhang's αβ stationary model. +2. Plant uses forward-Euler integration with `Ts = 100 µs`. +3. Inverter model is **ideal** (no dead time, no device drops) for simulation. +4. RTE hardware inverter remains duty-based; `control.mpcc` outputs switching states for future adapter to PWM. diff --git a/scripts/compare_mpcc_foc.py b/scripts/compare_mpcc_foc.py new file mode 100644 index 00000000..3be6c85d --- /dev/null +++ b/scripts/compare_mpcc_foc.py @@ -0,0 +1,93 @@ +#!/usr/bin/env python3 +"""Overlay MPCC (conventional/improved) vs FOC plots on Mac.""" + +from __future__ import annotations + +import csv +from pathlib import Path + +import matplotlib.pyplot as plt +import numpy as np + +ROOT = Path(__file__).resolve().parent.parent +DATA_DIR = ROOT / "results" / "comparison" +PLOT_DIR = ROOT / "results" / "comparison" / "plots" + + +def load_csv(path: Path) -> dict[str, np.ndarray]: + with path.open(newline="") as f: + rows = list(csv.DictReader(f)) + if not rows: + return {} + out: dict[str, np.ndarray] = {} + for k in rows[0]: + if k == "controller": + out[k] = np.array([row[k] for row in rows]) + else: + out[k] = np.array([float(row[k]) for row in rows]) + return out + + +def main() -> None: + mpcc = load_csv(DATA_DIR / "mpcc.csv") + mpcc_opt = load_csv(DATA_DIR / "mpcc_opt.csv") + foc = load_csv(DATA_DIR / "foc.csv") + if not mpcc or not mpcc_opt or not foc: + raise SystemExit( + "Missing CSV files. Run:\n" + " ./build/Lib/Simulation/compare_mpcc_foc\n" + "first." + ) + + PLOT_DIR.mkdir(parents=True, exist_ok=True) + t = mpcc["time"] + + fig, axes = plt.subplots(2, 2, figsize=(13, 9)) + fig.suptitle("FOC vs MPCC (Conventional/Optimal Duty) — load step 0→5 Nm at t=0.03 s, iq*=10 A") + + axes[0, 0].plot(t, foc["ia"], "--", label="FOC ia") + axes[0, 0].plot(t, mpcc["ia"], label="MPCC-conv ia") + axes[0, 0].plot(t, mpcc_opt["ia"], label="MPCC-opt ia", alpha=0.8) + axes[0, 0].set_ylabel("Phase current [A]") + axes[0, 0].legend(fontsize=8) + axes[0, 0].grid(True, alpha=0.3) + + axes[0, 1].plot(t, foc["id"], "--", label="FOC id") + axes[0, 1].plot(t, mpcc["id"], label="MPCC-conv id") + axes[0, 1].plot(t, mpcc_opt["id"], label="MPCC-opt id", alpha=0.8) + axes[0, 1].plot(t, foc["iq"], "--", label="FOC iq") + axes[0, 1].plot(t, mpcc["iq"], label="MPCC-conv iq") + axes[0, 1].plot(t, mpcc_opt["iq"], label="MPCC-opt iq", alpha=0.8) + axes[0, 1].plot(t, mpcc["id_reference"], "k:", label="id*") + axes[0, 1].plot(t, mpcc["iq_reference"], "k-.", label="iq*") + axes[0, 1].set_ylabel("dq current [A]") + axes[0, 1].legend(fontsize=8) + axes[0, 1].grid(True, alpha=0.3) + + axes[1, 0].plot(t, foc["electromagnetic_torque"], "--", label="FOC Te") + axes[1, 0].plot(t, mpcc["electromagnetic_torque"], label="MPCC-conv Te") + axes[1, 0].plot(t, mpcc_opt["electromagnetic_torque"], label="MPCC-opt Te", alpha=0.8) + axes[1, 0].plot(t, mpcc["load_torque"], "k:", label="Load Tl") + axes[1, 0].set_ylabel("Torque [Nm]") + axes[1, 0].set_xlabel("Time [s]") + axes[1, 0].legend(fontsize=8) + axes[1, 0].grid(True, alpha=0.3) + + axes[1, 1].plot(t, foc["mechanical_speed"], "--", label="FOC speed") + axes[1, 1].plot(t, mpcc["mechanical_speed"], label="MPCC-conv speed") + axes[1, 1].plot(t, mpcc_opt["mechanical_speed"], label="MPCC-opt speed", alpha=0.8) + axes[1, 1].set_ylabel("Mechanical speed [rad/s]") + axes[1, 1].set_xlabel("Time [s]") + axes[1, 1].legend(fontsize=8) + axes[1, 1].grid(True, alpha=0.3) + + fig.tight_layout() + out = PLOT_DIR / "mpcc_vs_foc_opt.png" + fig.savefig(out, dpi=150) + print(f"Saved comparison figure: {out}") + if plt.get_backend().lower() != "agg": + plt.show() + + +if __name__ == "__main__": + main() diff --git a/scripts/plot_results.py b/scripts/plot_results.py new file mode 100644 index 00000000..420a658b --- /dev/null +++ b/scripts/plot_results.py @@ -0,0 +1,117 @@ +#!/usr/bin/env python3 +"""Generate plots and performance metrics from MPC closed-loop CSV logs.""" + +from __future__ import annotations + +import csv +import math +from pathlib import Path + +import matplotlib.pyplot as plt +import numpy as np + +RESULTS_DIR = Path(__file__).resolve().parent.parent / "results" / "closed_loop" +PLOTS_DIR = Path(__file__).resolve().parent.parent / "results" / "plots" + + +def load_csv(path: Path) -> dict[str, np.ndarray]: + with path.open(newline="") as f: + reader = csv.DictReader(f) + rows = list(reader) + if not rows: + return {} + keys = rows[0].keys() + return {k: np.array([float(r[k]) for r in rows]) for k in keys} + + +def steady_state_slice(data: dict[str, np.ndarray], frac: float = 0.5) -> slice: + n = len(data["time"]) + return slice(int(n * frac), n) + + +def compute_metrics(data: dict[str, np.ndarray]) -> dict[str, float]: + ss = steady_state_slice(data) + id_err = data["id"][ss] - data["id_reference"][ss] + iq_err = data["iq"][ss] - data["iq_reference"][ss] + torque = data["electromagnetic_torque"][ss] + ia = data["ia"][ss] + metrics = { + "rms_id_error": float(np.sqrt(np.mean(id_err**2))), + "rms_iq_error": float(np.sqrt(np.mean(iq_err**2))), + "mean_abs_current_error": float(np.mean(np.abs(np.hstack([id_err, iq_err])))), + "max_current": float(np.max(np.sqrt(data["id"] ** 2 + data["iq"] ** 2))), + "torque_ripple_rms": float(np.std(torque)), + "torque_ripple_pp": float(np.max(torque) - np.min(torque)), + "mean_mpc_exec_us": float(np.mean(data["controller_execution_time"])), + "worst_mpc_exec_us": float(np.max(data["controller_execution_time"])), + } + # THD on phase-a steady-state current + x = ia - np.mean(ia) + n = len(x) + if n > 16: + spec = np.fft.rfft(x) + freqs = np.fft.rfftfreq(n, d=float(np.mean(np.diff(data["time"][ss])))) + fund_idx = np.argmax(np.abs(spec[1:])) + 1 + fund = np.abs(spec[fund_idx]) + harm = np.sqrt(np.sum(np.abs(spec[fund_idx + 1 :]) ** 2)) + metrics["phase_a_thd_pct"] = float(100.0 * harm / max(fund, 1e-9)) + return metrics + + +def plot_case(name: str, data: dict[str, np.ndarray]) -> None: + t = data["time"] + fig, axes = plt.subplots(4, 2, figsize=(14, 12)) + fig.suptitle(name) + + axes[0, 0].plot(t, data["ia"], label="ia") + axes[0, 0].plot(t, data["ib"], label="ib") + axes[0, 0].plot(t, data["ic"], label="ic") + axes[0, 0].legend(); axes[0, 0].set_ylabel("A") + + axes[0, 1].plot(t, data["id"], label="id") + axes[0, 1].plot(t, data["id_reference"], "--", label="id*") + axes[0, 1].plot(t, data["iq"], label="iq") + axes[0, 1].plot(t, data["iq_reference"], "--", label="iq*") + axes[0, 1].legend(); axes[0, 1].set_ylabel("A") + + axes[1, 0].plot(t, data["id"] - data["id_reference"]) + axes[1, 0].set_ylabel("id error") + axes[1, 1].plot(t, data["iq"] - data["iq_reference"]) + axes[1, 1].set_ylabel("iq error") + + axes[2, 0].plot(t, data["mechanical_speed"]) + axes[2, 0].set_ylabel("rad/s") + axes[2, 1].plot(t, data["electromagnetic_torque"], label="Te") + axes[2, 1].plot(t, data["load_torque"], label="Tl") + axes[2, 1].legend(); axes[2, 1].set_ylabel("Nm") + + axes[3, 0].plot(t, data["switching_state"]) + axes[3, 0].set_ylabel("state") + axes[3, 1].plot(t, data["cost"]) + axes[3, 1].set_ylabel("cost") + + for ax in axes[-1]: + ax.set_xlabel("time (s)") + fig.tight_layout() + PLOTS_DIR.mkdir(parents=True, exist_ok=True) + fig.savefig(PLOTS_DIR / f"{name}.png", dpi=150) + plt.close(fig) + + +def main() -> None: + summary_lines = ["case,metric,value"] + for csv_path in sorted(RESULTS_DIR.glob("*.csv")): + data = load_csv(csv_path) + if not data: + continue + plot_case(csv_path.stem, data) + metrics = compute_metrics(data) + for k, v in metrics.items(): + summary_lines.append(f"{csv_path.stem},{k},{v:.6g}") + out = Path(__file__).resolve().parent.parent / "results" / "metrics_summary.csv" + out.write_text("\n".join(summary_lines) + "\n") + print(f"Wrote plots to {PLOTS_DIR} and metrics to {out}") + + +if __name__ == "__main__": + main()