motor_ctl-4/tests/unit_test/motor_sim/PmsmSimUt.cpp

//
// File: PmsmSimUt.cpp
//
// Code generated for Simulink model 'PmsmSimUt'.
//
// Model version                  : 1.18
// Simulink Coder version         : 9.4 (R2020b) 29-Jul-2020
// C/C++ source code generated on : Tue Aug  1 23:42:37 2023
//
// Target selection: ert.tlc
// Embedded hardware selection: Intel->x86-64 (Windows64)
// Code generation objectives: Unspecified
// Validation result: Not run
//
#include "PmsmSimUt.h"
#include "PmsmSimUt_private.h"

void PmsmSimUtModelClass::PmsmSimUt_reset()
{
  // Outputs for Atomic SubSystem: '<Root>/Initialize Function'
  // StateWriter: '<S2>/State Writer' incorporates:
  //   Constant: '<S2>/Constant4'

  PmsmSimUt_DW.DiscreteTimeIntegrator_DSTATE_c = PmsmSimUt_P.Params.Flux;

  // End of Outputs for SubSystem: '<Root>/Initialize Function'
}

real_T rt_modd_snf(real_T u0, real_T u1)
{
  real_T q;
  real_T y;
  boolean_T yEq;
  y = u0;
  if (u1 == 0.0) {
    if (u0 == 0.0) {
      y = u1;
    }
  } else if (rtIsNaN(u0) || rtIsNaN(u1) || rtIsInf(u0)) {
    y = (rtNaN);
  } else if (u0 == 0.0) {
    y = 0.0 / u1;
  } else if (rtIsInf(u1)) {
    if ((u1 < 0.0) != (u0 < 0.0)) {
      y = u1;
    }
  } else {
    y = std::fmod(u0, u1);
    yEq = (y == 0.0);
    if ((!yEq) && (u1 > std::floor(u1))) {
      q = std::abs(u0 / u1);
      yEq = !(std::abs(q - std::floor(q + 0.5)) > DBL_EPSILON * q);
    }

    if (yEq) {
      y = u1 * 0.0;
    } else {
      if ((u0 < 0.0) != (u1 < 0.0)) {
        y += u1;
      }
    }
  }

  return y;
}

// Model step function
void PmsmSimUtModelClass::step()
{
  real_T rtb_Gain1_c[3];
  real_T rtb_Add1;
  real_T rtb_Add2;
  real_T rtb_Add2_tmp_tmp;
  real_T rtb_Add2_tmp_tmp_0;
  real_T rtb_Fcn1;
  real_T rtb_Gain;
  real_T rtb_Gain1_i;
  real_T rtb_Gain2_k;
  real_T rtb_Mod;
  real_T rtb_Relay2;
  real_T rtb_Switch_h;
  real_T rtb_Switch_idx_0;
  real_T rtb_Switch_idx_1;
  real_T rtb_a;
  real_T rtb_id;
  real_T rtb_iq;
  int32_T i;
  uint8_T rtb_Compare;

  // Math: '<S18>/Mod' incorporates:
  //   Constant: '<S18>/Constant'
  //   DiscreteIntegrator: '<S18>/Discrete-Time Integrator'

  rtb_Mod = rt_modd_snf(PmsmSimUt_DW.DiscreteTimeIntegrator_DSTATE,
                        PmsmSimUt_P.Constant_Value);

  // Fcn: '<S17>/iq' incorporates:
  //   Constant: '<S17>/Constant3'
  //   DiscreteIntegrator: '<S17>/Discrete-Time Integrator1'

  rtb_iq = PmsmSimUt_DW.DiscreteTimeIntegrator1_DSTATE / PmsmSimUt_P.Params.Lq;

  // Fcn: '<S17>/id' incorporates:
  //   Constant: '<S17>/Constant1'
  //   Constant: '<S17>/Constant2'
  //   DiscreteIntegrator: '<S17>/Discrete-Time Integrator'

  rtb_id = (PmsmSimUt_DW.DiscreteTimeIntegrator_DSTATE_c -
            PmsmSimUt_P.Params.Flux) / PmsmSimUt_P.Params.Ld;

  // Outputs for Enabled SubSystem: '<S21>/Subsystem - pi//2 delay' incorporates:
  //   EnablePort: '<S25>/Enable'

  // Outputs for Enabled SubSystem: '<S21>/Subsystem1' incorporates:
  //   EnablePort: '<S26>/Enable'

  // Fcn: '<S29>/Fcn' incorporates:
  //   Fcn: '<S25>/Fcn'
  //   Fcn: '<S25>/Fcn1'
  //   Fcn: '<S26>/Fcn'

  rtb_Add2_tmp_tmp = std::sin(rtb_Mod);
  rtb_Add2_tmp_tmp_0 = std::cos(rtb_Mod);

  // End of Outputs for SubSystem: '<S21>/Subsystem1'
  // End of Outputs for SubSystem: '<S21>/Subsystem - pi//2 delay'
  rtb_Add2 = rtb_Add2_tmp_tmp_0 * rtb_id - rtb_Add2_tmp_tmp * rtb_iq;

  // Fcn: '<S29>/Fcn1' incorporates:
  //   Fcn: '<S29>/Fcn'

  rtb_a = rtb_Add2_tmp_tmp * rtb_id + rtb_Add2_tmp_tmp_0 * rtb_iq;

  // Gain: '<S30>/K1' incorporates:
  //   Constant: '<S10>/Constant'

  rtb_Gain1_i = PmsmSimUt_P.K1_Gain * PmsmSimUt_P.Constant_Value_e;

  // Fcn: '<S30>/a'
  rtb_Gain2_k = rtb_Add2 + rtb_Gain1_i;

  // Fcn: '<S30>/b'
  rtb_Fcn1 = (-0.5 * rtb_Add2 + 0.8660254037844386 * rtb_a) + rtb_Gain1_i;

  // Fcn: '<S30>/c'
  rtb_Add2 = (-0.5 * rtb_Add2 - 0.8660254037844386 * rtb_a) + rtb_Gain1_i;

  // Outport: '<Root>/Out' incorporates:
  //   Gain: '<S30>/K2'

  PmsmSimUt_Y.Out.Iabc[0] = PmsmSimUt_P.K2_Gain * rtb_Gain2_k;
  PmsmSimUt_Y.Out.Iabc[1] = PmsmSimUt_P.K2_Gain * rtb_Fcn1;
  PmsmSimUt_Y.Out.Iabc[2] = PmsmSimUt_P.K2_Gain * rtb_Add2;

  // Switch: '<Root>/Switch2' incorporates:
  //   Inport: '<Root>/ObsIn'

  if (PmsmSimUt_U.ObsIn.Enable > PmsmSimUt_P.Switch2_Threshold) {
    rtb_Fcn1 = PmsmSimUt_U.ObsIn.Theta;
  } else {
    rtb_Fcn1 = rtb_Mod;
  }

  // End of Switch: '<Root>/Switch2'

  // Gain: '<S17>/Gain' incorporates:
  //   DiscreteIntegrator: '<S17>/Discrete-Time Integrator'
  //   DiscreteIntegrator: '<S17>/Discrete-Time Integrator1'
  //   Fcn: '<S17>/Te//p=(3//2)*(Flux_d*iq-Flux_q*id)'

  rtb_Gain = (PmsmSimUt_DW.DiscreteTimeIntegrator_DSTATE_c * rtb_iq -
              PmsmSimUt_DW.DiscreteTimeIntegrator1_DSTATE * rtb_id) * 1.5 *
    PmsmSimUt_P.Params.Pn;

  // Outputs for Enabled SubSystem: '<S18>/Subsystem' incorporates:
  //   EnablePort: '<S19>/Enable'

  // Constant: '<Root>/Constant2'
  if (PmsmSimUt_P.Params.SpdCtrl > 0.0) {
    // DiscreteIntegrator: '<S19>/Discrete-Time Integrator1'
    PmsmSimUt_B.DiscreteTimeIntegrator1 =
      PmsmSimUt_DW.DiscreteTimeIntegrator1_DSTAT_d;

    // Update for DiscreteIntegrator: '<S19>/Discrete-Time Integrator1' incorporates:
    //   Constant: '<S19>/Constant1'
    //   Gain: '<S19>/Gain'
    //   Gain: '<S19>/Gain1'
    //   Inport: '<Root>/CtrlIn'
    //   Product: '<S19>/Divide'
    //   Sum: '<S19>/Sum'

    PmsmSimUt_DW.DiscreteTimeIntegrator1_DSTAT_d += ((rtb_Gain -
      PmsmSimUt_P.Params.B * PmsmSimUt_B.DiscreteTimeIntegrator1) -
      PmsmSimUt_U.CtrlIn.Tm) / PmsmSimUt_P.Params.Jm * PmsmSimUt_P.Params.Ts *
      PmsmSimUt_P.DiscreteTimeIntegrator1_gainval;
  }

  // End of Outputs for SubSystem: '<S18>/Subsystem'

  // Switch: '<S18>/Switch' incorporates:
  //   Constant: '<Root>/Constant2'
  //   Constant: '<Root>/Constant4'
  //   Gain: '<S18>/Gain3'

  if (PmsmSimUt_P.Params.SpdCtrl > PmsmSimUt_P.Switch_Threshold) {
    rtb_Gain2_k = PmsmSimUt_B.DiscreteTimeIntegrator1;
  } else {
    rtb_Gain2_k = PmsmSimUt_P.Gain3_Gain * PmsmSimUt_P.Params.SpdRpm;
  }

  // End of Switch: '<S18>/Switch'

  // Gain: '<S18>/Gain1'
  rtb_Gain1_i = PmsmSimUt_P.Params.Pn * rtb_Gain2_k;

  // Switch: '<Root>/Switch3' incorporates:
  //   Inport: '<Root>/ObsIn'

  if (PmsmSimUt_U.ObsIn.Enable > PmsmSimUt_P.Switch3_Threshold) {
    rtb_a = PmsmSimUt_U.ObsIn.We;
  } else {
    rtb_a = rtb_Gain1_i;
  }

  // End of Switch: '<Root>/Switch3'

  // RelationalOperator: '<S11>/Compare' incorporates:
  //   Constant: '<S11>/Constant'
  //   Constant: '<S1>/Constant'

  rtb_Compare = (PmsmSimUt_P.AlphaBetaZerotodq0_Alignment ==
                 PmsmSimUt_P.CompareToConstant_const);

  // Sum: '<Root>/Add' incorporates:
  //   Inport: '<Root>/ObsIn'

  rtb_Add2 = PmsmSimUt_U.ObsIn.Theta - rtb_Mod;

  // Outputs for Enabled SubSystem: '<S1>/Subsystem1' incorporates:
  //   EnablePort: '<S14>/Enable'

  if (rtb_Compare > 0) {
    // Fcn: '<S14>/Fcn' incorporates:
    //   Fcn: '<S14>/Fcn1'

    rtb_Add1 = std::sin(rtb_Add2);
    rtb_Switch_h = std::cos(rtb_Add2);

    // Fcn: '<S14>/Fcn'
    PmsmSimUt_B.Fcn_g = rtb_id * rtb_Switch_h + rtb_iq * rtb_Add1;

    // Fcn: '<S14>/Fcn1'
    PmsmSimUt_B.Fcn1_e = -rtb_id * rtb_Add1 + rtb_iq * rtb_Switch_h;
  }

  // End of Outputs for SubSystem: '<S1>/Subsystem1'

  // Outputs for Enabled SubSystem: '<S1>/Subsystem - pi//2 delay' incorporates:
  //   EnablePort: '<S13>/Enable'

  // RelationalOperator: '<S12>/Compare' incorporates:
  //   Constant: '<S12>/Constant'
  //   Constant: '<S1>/Constant'

  if (PmsmSimUt_P.AlphaBetaZerotodq0_Alignment ==
      PmsmSimUt_P.CompareToConstant1_const) {
    // Fcn: '<S13>/Fcn'
    PmsmSimUt_B.Fcn_b = rtb_id * std::sin(rtb_Add2) - rtb_iq * std::cos(rtb_Add2);

    // Fcn: '<S13>/Fcn1'
    PmsmSimUt_B.Fcn1_n = rtb_id * std::cos(rtb_Add2) + rtb_iq * std::sin
      (rtb_Add2);
  }

  // End of RelationalOperator: '<S12>/Compare'
  // End of Outputs for SubSystem: '<S1>/Subsystem - pi//2 delay'

  // Switch: '<S1>/Switch'
  if (rtb_Compare != 0) {
    rtb_Switch_idx_0 = PmsmSimUt_B.Fcn_g;
    rtb_Switch_idx_1 = PmsmSimUt_B.Fcn1_e;
  } else {
    rtb_Switch_idx_0 = PmsmSimUt_B.Fcn_b;
    rtb_Switch_idx_1 = PmsmSimUt_B.Fcn1_n;
  }

  // End of Switch: '<S1>/Switch'

  // Switch: '<Root>/Switch5' incorporates:
  //   Inport: '<Root>/ObsIn'

  if (PmsmSimUt_U.ObsIn.Enable > PmsmSimUt_P.Switch5_Threshold) {
    rtb_Add2 = rtb_Switch_idx_1;
  } else {
    rtb_Add2 = rtb_iq;
  }

  // End of Switch: '<Root>/Switch5'

  // Switch: '<Root>/Switch4' incorporates:
  //   Inport: '<Root>/ObsIn'

  if (PmsmSimUt_U.ObsIn.Enable > PmsmSimUt_P.Switch4_Threshold) {
    rtb_Add1 = rtb_Switch_idx_0;
  } else {
    rtb_Add1 = rtb_id;
  }

  // End of Switch: '<Root>/Switch4'

  // ManualSwitch: '<Root>/Manual Switch1' incorporates:
  //   Constant: '<Root>/Constant5'
  //   Constant: '<Root>/Constant6'

  if (PmsmSimUt_P.ManualSwitch1_CurrentSetting == 1) {
    rtb_Switch_idx_0 = PmsmSimUt_P.Constant5_Value;
  } else {
    rtb_Switch_idx_0 = PmsmSimUt_P.Constant6_Value;
  }

  // End of ManualSwitch: '<Root>/Manual Switch1'

  // Product: '<S6>/Product' incorporates:
  //   Constant: '<S6>/Constant1'
  //   Constant: '<S6>/Constant2'
  //   Product: '<S6>/Product2'
  //   Product: '<S6>/Product3'
  //   Sum: '<S6>/Sum'

  rtb_Switch_idx_1 = (rtb_Add1 * PmsmSimUt_P.Params.Ld + PmsmSimUt_P.Params.Flux)
    * rtb_a * rtb_Switch_idx_0;

  // Sum: '<Root>/Add1' incorporates:
  //   Inport: '<Root>/CtrlIn'

  rtb_Add1 = PmsmSimUt_U.CtrlIn.IdCmd - rtb_Add1;

  // Switch: '<Root>/Switch1' incorporates:
  //   Constant: '<S6>/Constant3'
  //   Gain: '<S6>/Gain1'
  //   Inport: '<Root>/ICtrlIn'
  //   Product: '<S6>/Product'
  //   Product: '<S6>/Product1'
  //   Sum: '<Root>/Sum1'

  if (PmsmSimUt_U.ICtrlIn.Enable > PmsmSimUt_P.Switch1_Threshold) {
    rtb_Switch_idx_0 = PmsmSimUt_U.ICtrlIn.UdCtrl;
  } else {
    // Sum: '<S16>/Sum6' incorporates:
    //   DiscreteIntegrator: '<S16>/Discrete-Time Integrator'
    //   Gain: '<S16>/Kp4'

    rtb_Relay2 = PmsmSimUt_P.Params.CKpd * rtb_Add1 +
      PmsmSimUt_DW.DiscreteTimeIntegrator_DSTATE_j;

    // Saturate: '<S16>/Saturation2'
    if (rtb_Relay2 > PmsmSimUt_P.Saturation2_UpperSat_o) {
      rtb_Relay2 = PmsmSimUt_P.Saturation2_UpperSat_o;
    } else {
      if (rtb_Relay2 < PmsmSimUt_P.Saturation2_LowerSat_o) {
        rtb_Relay2 = PmsmSimUt_P.Saturation2_LowerSat_o;
      }
    }

    // End of Saturate: '<S16>/Saturation2'
    rtb_Switch_idx_0 = PmsmSimUt_P.Gain1_Gain * rtb_a * rtb_Add2 *
      PmsmSimUt_P.Params.Lq * rtb_Switch_idx_0 + rtb_Relay2;
  }

  // End of Switch: '<Root>/Switch1'

  // Outputs for Enabled SubSystem: '<Root>/Subsystem' incorporates:
  //   EnablePort: '<S5>/Enable'

  // Logic: '<Root>/AND' incorporates:
  //   Constant: '<Root>/Constant1'
  //   Constant: '<Root>/Constant3'
  //   Logic: '<Root>/NOT'

  if ((!(PmsmSimUt_P.Params.CustomSpdCtrl != 0.0)) &&
      (PmsmSimUt_P.Params.SpdCtrl != 0.0)) {
    PmsmSimUt_DW.Subsystem_MODE = true;

    // Saturate: '<Root>/Saturation2' incorporates:
    //   Memory: '<S7>/Memory'

    if (PmsmSimUt_DW.Memory_PreviousInput > PmsmSimUt_P.Saturation2_UpperSat_p)
    {
      rtb_Switch_h = PmsmSimUt_P.Saturation2_UpperSat_p;
    } else if (PmsmSimUt_DW.Memory_PreviousInput <
               PmsmSimUt_P.Saturation2_LowerSat_f) {
      rtb_Switch_h = PmsmSimUt_P.Saturation2_LowerSat_f;
    } else {
      rtb_Switch_h = PmsmSimUt_DW.Memory_PreviousInput;
    }

    // End of Saturate: '<Root>/Saturation2'

    // Sum: '<S5>/Add4' incorporates:
    //   Gain: '<Root>/Gain1'

    rtb_Relay2 = rtb_Switch_h - 9.5492965855137211 / PmsmSimUt_P.Params.Pn *
      rtb_a;

    // DiscreteIntegrator: '<S20>/Discrete-Time Integrator'
    PmsmSimUt_B.DiscreteTimeIntegrator =
      PmsmSimUt_DW.DiscreteTimeIntegrator_DSTATE_n;

    // Sum: '<S20>/Sum6' incorporates:
    //   Gain: '<S20>/Kp4'

    PmsmSimUt_B.Iq_ref = PmsmSimUt_P.Params.SKp * rtb_Relay2 +
      PmsmSimUt_B.DiscreteTimeIntegrator;

    // Saturate: '<S20>/Saturation2'
    if (PmsmSimUt_B.Iq_ref > PmsmSimUt_P.Saturation2_UpperSat) {
      // Sum: '<S20>/Sum6' incorporates:
      //   Saturate: '<S20>/Saturation2'

      PmsmSimUt_B.Iq_ref = PmsmSimUt_P.Saturation2_UpperSat;
    } else {
      if (PmsmSimUt_B.Iq_ref < PmsmSimUt_P.Saturation2_LowerSat) {
        // Sum: '<S20>/Sum6' incorporates:
        //   Saturate: '<S20>/Saturation2'

        PmsmSimUt_B.Iq_ref = PmsmSimUt_P.Saturation2_LowerSat;
      }
    }

    // End of Saturate: '<S20>/Saturation2'

    // Update for DiscreteIntegrator: '<S20>/Discrete-Time Integrator' incorporates:
    //   Gain: '<S20>/Gain'
    //   Gain: '<S20>/Kp5'

    PmsmSimUt_DW.DiscreteTimeIntegrator_DSTATE_n += PmsmSimUt_P.Params.SKi *
      rtb_Relay2 * PmsmSimUt_P.Params.Ts *
      PmsmSimUt_P.DiscreteTimeIntegrator_gainval;
    if (PmsmSimUt_DW.DiscreteTimeIntegrator_DSTATE_n >=
        PmsmSimUt_P.DiscreteTimeIntegrator_UpperSat) {
      PmsmSimUt_DW.DiscreteTimeIntegrator_DSTATE_n =
        PmsmSimUt_P.DiscreteTimeIntegrator_UpperSat;
    } else {
      if (PmsmSimUt_DW.DiscreteTimeIntegrator_DSTATE_n <=
          PmsmSimUt_P.DiscreteTimeIntegrator_LowerSat) {
        PmsmSimUt_DW.DiscreteTimeIntegrator_DSTATE_n =
          PmsmSimUt_P.DiscreteTimeIntegrator_LowerSat;
      }
    }

    // End of Update for DiscreteIntegrator: '<S20>/Discrete-Time Integrator'

    // Switch: '<Root>/Switch'
    rtb_Switch_h = PmsmSimUt_B.Iq_ref;
  } else {
    if (PmsmSimUt_DW.Subsystem_MODE) {
      // Disable for DiscreteIntegrator: '<S20>/Discrete-Time Integrator'
      PmsmSimUt_DW.DiscreteTimeIntegrator_DSTATE_n =
        PmsmSimUt_B.DiscreteTimeIntegrator;
      PmsmSimUt_DW.Subsystem_MODE = false;
    }

    // Switch: '<Root>/Switch' incorporates:
    //   Inport: '<Root>/CtrlIn'

    rtb_Switch_h = PmsmSimUt_U.CtrlIn.IqCmd;
  }

  // End of Logic: '<Root>/AND'
  // End of Outputs for SubSystem: '<Root>/Subsystem'

  // Sum: '<Root>/Add2'
  rtb_Add2 = rtb_Switch_h - rtb_Add2;

  // Switch: '<Root>/Switch6' incorporates:
  //   Inport: '<Root>/ICtrlIn'
  //   Sum: '<Root>/Sum'

  if (PmsmSimUt_U.ICtrlIn.Enable > PmsmSimUt_P.Switch6_Threshold) {
    rtb_a = PmsmSimUt_U.ICtrlIn.UqCtrl;
  } else {
    // Sum: '<S15>/Sum6' incorporates:
    //   DiscreteIntegrator: '<S15>/Discrete-Time Integrator'
    //   Gain: '<S15>/Kp4'

    rtb_Relay2 = PmsmSimUt_P.Params.CKpq * rtb_Add2 +
      PmsmSimUt_DW.DiscreteTimeIntegrator_DSTATE_g;

    // Saturate: '<S15>/Saturation2'
    if (rtb_Relay2 > PmsmSimUt_P.Saturation2_UpperSat_b) {
      rtb_Relay2 = PmsmSimUt_P.Saturation2_UpperSat_b;
    } else {
      if (rtb_Relay2 < PmsmSimUt_P.Saturation2_LowerSat_j) {
        rtb_Relay2 = PmsmSimUt_P.Saturation2_LowerSat_j;
      }
    }

    // End of Saturate: '<S15>/Saturation2'
    rtb_a = rtb_Switch_idx_1 + rtb_Relay2;
  }

  // End of Switch: '<Root>/Switch6'

  // Fcn: '<S27>/Fcn' incorporates:
  //   Fcn: '<S27>/Fcn1'

  rtb_Relay2 = std::sin(rtb_Fcn1);
  rtb_Fcn1 = std::cos(rtb_Fcn1);
  rtb_Switch_idx_1 = rtb_Fcn1 * rtb_Switch_idx_0 - rtb_Relay2 * rtb_a;

  // Fcn: '<S27>/Fcn1'
  rtb_Fcn1 = rtb_Relay2 * rtb_Switch_idx_0 + rtb_Fcn1 * rtb_a;

  // Gain: '<S28>/K1' incorporates:
  //   Constant: '<S9>/Constant'

  rtb_Switch_idx_0 = PmsmSimUt_P.K1_Gain_j * PmsmSimUt_P.Constant_Value_p;

  // Fcn: '<S28>/a'
  rtb_a = rtb_Switch_idx_1 + rtb_Switch_idx_0;

  // Fcn: '<S28>/b'
  rtb_Relay2 = (-0.5 * rtb_Switch_idx_1 + 0.8660254037844386 * rtb_Fcn1) +
    rtb_Switch_idx_0;

  // Fcn: '<S28>/c'
  rtb_Switch_idx_1 = (-0.5 * rtb_Switch_idx_1 - 0.8660254037844386 * rtb_Fcn1) +
    rtb_Switch_idx_0;

  // Gain: '<S28>/K2'
  rtb_a *= PmsmSimUt_P.K2_Gain_b;
  rtb_Fcn1 = PmsmSimUt_P.K2_Gain_b * rtb_Relay2;
  rtb_Relay2 = PmsmSimUt_P.K2_Gain_b * rtb_Switch_idx_1;
  for (i = 0; i < 3; i++) {
    // Gain: '<S22>/Gain1' incorporates:
    //   Gain: '<S22>/Gain3'

    rtb_Gain1_c[i] = PmsmSimUt_P.Gain1_Gain_e * (PmsmSimUt_P.Gain3_Gain_j[i + 6]
      * rtb_Relay2 + (PmsmSimUt_P.Gain3_Gain_j[i + 3] * rtb_Fcn1 +
                      PmsmSimUt_P.Gain3_Gain_j[i] * rtb_a));
  }

  // RelationalOperator: '<S23>/Compare' incorporates:
  //   Constant: '<S21>/Constant'
  //   Constant: '<S23>/Constant'

  rtb_Compare = (PmsmSimUt_P.AlphaBetaZerotodq0_Alignment_e ==
                 PmsmSimUt_P.CompareToConstant_const_a);

  // Outputs for Enabled SubSystem: '<S21>/Subsystem1' incorporates:
  //   EnablePort: '<S26>/Enable'

  if (rtb_Compare > 0) {
    // Fcn: '<S26>/Fcn'
    PmsmSimUt_B.Fcn = rtb_Gain1_c[0] * rtb_Add2_tmp_tmp_0 + rtb_Gain1_c[1] *
      rtb_Add2_tmp_tmp;

    // Fcn: '<S26>/Fcn1'
    PmsmSimUt_B.Fcn1 = -rtb_Gain1_c[0] * rtb_Add2_tmp_tmp + rtb_Gain1_c[1] *
      rtb_Add2_tmp_tmp_0;
  }

  // End of Outputs for SubSystem: '<S21>/Subsystem1'

  // Outputs for Enabled SubSystem: '<S21>/Subsystem - pi//2 delay' incorporates:
  //   EnablePort: '<S25>/Enable'

  // RelationalOperator: '<S24>/Compare' incorporates:
  //   Constant: '<S21>/Constant'
  //   Constant: '<S24>/Constant'

  if (PmsmSimUt_P.AlphaBetaZerotodq0_Alignment_e ==
      PmsmSimUt_P.CompareToConstant1_const_o) {
    // Fcn: '<S25>/Fcn'
    PmsmSimUt_B.Fcn_a = rtb_Gain1_c[0] * rtb_Add2_tmp_tmp - rtb_Gain1_c[1] *
      rtb_Add2_tmp_tmp_0;

    // Fcn: '<S25>/Fcn1'
    PmsmSimUt_B.Fcn1_p = rtb_Gain1_c[0] * rtb_Add2_tmp_tmp_0 + rtb_Gain1_c[1] *
      rtb_Add2_tmp_tmp;
  }

  // End of RelationalOperator: '<S24>/Compare'
  // End of Outputs for SubSystem: '<S21>/Subsystem - pi//2 delay'

  // Switch: '<S21>/Switch'
  if (rtb_Compare != 0) {
    rtb_Switch_idx_0 = PmsmSimUt_B.Fcn;
    rtb_Switch_idx_1 = PmsmSimUt_B.Fcn1;
  } else {
    rtb_Switch_idx_0 = PmsmSimUt_B.Fcn_a;
    rtb_Switch_idx_1 = PmsmSimUt_B.Fcn1_p;
  }

  // End of Switch: '<S21>/Switch'

  // Outport: '<Root>/Out' incorporates:
  //   BusCreator: '<Root>/Bus Creator'
  //   DiscreteIntegrator: '<S17>/Discrete-Time Integrator'
  //   DiscreteIntegrator: '<S17>/Discrete-Time Integrator1'
  //   Gain: '<S18>/Gain2'
  //   Inport: '<Root>/CtrlIn'
  //   SignalConversion generated from: '<Root>/Bus Creator'

  PmsmSimUt_Y.Out.WmRpm = PmsmSimUt_P.Gain2_Gain * rtb_Gain2_k;
  PmsmSimUt_Y.Out.Uabc[0] = rtb_a;
  PmsmSimUt_Y.Out.Uabc[1] = rtb_Fcn1;
  PmsmSimUt_Y.Out.Uabc[2] = rtb_Relay2;
  PmsmSimUt_Y.Out.Idq[0] = rtb_id;
  PmsmSimUt_Y.Out.Idq[1] = rtb_iq;
  PmsmSimUt_Y.Out.Te = rtb_Gain;
  PmsmSimUt_Y.Out.Theta = rtb_Mod;
  PmsmSimUt_Y.Out.We = rtb_Gain1_i;
  PmsmSimUt_Y.Out.FluxDq[0] = PmsmSimUt_DW.DiscreteTimeIntegrator_DSTATE_c;
  PmsmSimUt_Y.Out.FluxDq[1] = PmsmSimUt_DW.DiscreteTimeIntegrator1_DSTATE;
  PmsmSimUt_Y.Out.IdRef = PmsmSimUt_U.CtrlIn.IdCmd;
  PmsmSimUt_Y.Out.IqRef = rtb_Switch_h;

  // Gain: '<S17>/Gain1' incorporates:
  //   Constant: '<S17>/Constant'
  //   DiscreteIntegrator: '<S17>/Discrete-Time Integrator1'
  //   Fcn: '<S17>/d(Flux_d)//dt'

  rtb_Mod = ((rtb_Switch_idx_0 - PmsmSimUt_P.Params.R * rtb_id) + rtb_Gain1_i *
             PmsmSimUt_DW.DiscreteTimeIntegrator1_DSTATE) *
    PmsmSimUt_P.Params.Ts;

  // Sum: '<S7>/Sum' incorporates:
  //   Inport: '<Root>/CtrlIn'
  //   Memory: '<S7>/Memory'

  rtb_Relay2 = PmsmSimUt_U.CtrlIn.WmRpm - PmsmSimUt_DW.Memory_PreviousInput;

  // DeadZone: '<S7>/Dead Zone'
  if (rtb_Relay2 > PmsmSimUt_P.DeadZone_End) {
    rtb_Relay2 -= PmsmSimUt_P.DeadZone_End;
  } else if (rtb_Relay2 >= PmsmSimUt_P.DeadZone_Start) {
    rtb_Relay2 = 0.0;
  } else {
    rtb_Relay2 -= PmsmSimUt_P.DeadZone_Start;
  }

  // End of DeadZone: '<S7>/Dead Zone'

  // Relay: '<S7>/Relay1'
  PmsmSimUt_DW.Relay1_Mode = ((rtb_Relay2 >= PmsmSimUt_P.Relay1_OnVal) ||
    ((!(rtb_Relay2 <= PmsmSimUt_P.Relay1_OffVal)) && PmsmSimUt_DW.Relay1_Mode));

  // Relay: '<S7>/Relay2'
  PmsmSimUt_DW.Relay2_Mode = ((rtb_Relay2 >= PmsmSimUt_P.Relay2_OnVal) ||
    ((!(rtb_Relay2 <= PmsmSimUt_P.Relay2_OffVal)) && PmsmSimUt_DW.Relay2_Mode));

  // Update for DiscreteIntegrator: '<S18>/Discrete-Time Integrator' incorporates:
  //   Gain: '<S18>/Gain'

  PmsmSimUt_DW.DiscreteTimeIntegrator_DSTATE += PmsmSimUt_P.Params.Ts *
    rtb_Gain1_i * PmsmSimUt_P.DiscreteTimeIntegrator_gainva_p;

  // Update for DiscreteIntegrator: '<S17>/Discrete-Time Integrator1' incorporates:
  //   Constant: '<S17>/Constant'
  //   DiscreteIntegrator: '<S17>/Discrete-Time Integrator'
  //   Fcn: '<S17>/d(Flux_q)//dt'
  //   Gain: '<S17>/Gain2'

  PmsmSimUt_DW.DiscreteTimeIntegrator1_DSTATE += ((rtb_Switch_idx_1 -
    PmsmSimUt_P.Params.R * rtb_iq) - rtb_Gain1_i *
    PmsmSimUt_DW.DiscreteTimeIntegrator_DSTATE_c) * PmsmSimUt_P.Params.Ts *
    PmsmSimUt_P.DiscreteTimeIntegrator1_gainv_b;

  // Update for DiscreteIntegrator: '<S17>/Discrete-Time Integrator'
  PmsmSimUt_DW.DiscreteTimeIntegrator_DSTATE_c +=
    PmsmSimUt_P.DiscreteTimeIntegrator_gainv_pq * rtb_Mod;

  // Update for DiscreteIntegrator: '<S16>/Discrete-Time Integrator' incorporates:
  //   Gain: '<S16>/Gain'
  //   Gain: '<S16>/Kp5'

  PmsmSimUt_DW.DiscreteTimeIntegrator_DSTATE_j += PmsmSimUt_P.Params.CKid *
    rtb_Add1 * PmsmSimUt_P.Params.Ts *
    PmsmSimUt_P.DiscreteTimeIntegrator_gainva_b;
  if (PmsmSimUt_DW.DiscreteTimeIntegrator_DSTATE_j >=
      PmsmSimUt_P.DiscreteTimeIntegrator_UpperS_b) {
    PmsmSimUt_DW.DiscreteTimeIntegrator_DSTATE_j =
      PmsmSimUt_P.DiscreteTimeIntegrator_UpperS_b;
  } else {
    if (PmsmSimUt_DW.DiscreteTimeIntegrator_DSTATE_j <=
        PmsmSimUt_P.DiscreteTimeIntegrator_LowerS_l) {
      PmsmSimUt_DW.DiscreteTimeIntegrator_DSTATE_j =
        PmsmSimUt_P.DiscreteTimeIntegrator_LowerS_l;
    }
  }

  // End of Update for DiscreteIntegrator: '<S16>/Discrete-Time Integrator'

  // Relay: '<S7>/Relay1'
  if (PmsmSimUt_DW.Relay1_Mode) {
    rtb_Switch_h = PmsmSimUt_P.Relay1_YOn;
  } else {
    rtb_Switch_h = PmsmSimUt_P.Relay1_YOff;
  }

  // Relay: '<S7>/Relay2'
  if (PmsmSimUt_DW.Relay2_Mode) {
    rtb_iq = PmsmSimUt_P.Relay2_YOn;
  } else {
    rtb_iq = PmsmSimUt_P.Relay2_YOff;
  }

  // Update for Memory: '<S7>/Memory' incorporates:
  //   Gain: '<S7>/Gain'
  //   Sum: '<S7>/Add'
  //   Sum: '<S7>/Sum1'

  PmsmSimUt_DW.Memory_PreviousInput += (rtb_Switch_h + rtb_iq) *
    PmsmSimUt_P.Params.Ts;

  // Update for DiscreteIntegrator: '<S15>/Discrete-Time Integrator' incorporates:
  //   Gain: '<S15>/Gain'
  //   Gain: '<S15>/Kp5'

  PmsmSimUt_DW.DiscreteTimeIntegrator_DSTATE_g += PmsmSimUt_P.Params.CKiq *
    rtb_Add2 * PmsmSimUt_P.Params.Ts *
    PmsmSimUt_P.DiscreteTimeIntegrator_gainva_c;
  if (PmsmSimUt_DW.DiscreteTimeIntegrator_DSTATE_g >=
      PmsmSimUt_P.DiscreteTimeIntegrator_UpperS_k) {
    PmsmSimUt_DW.DiscreteTimeIntegrator_DSTATE_g =
      PmsmSimUt_P.DiscreteTimeIntegrator_UpperS_k;
  } else {
    if (PmsmSimUt_DW.DiscreteTimeIntegrator_DSTATE_g <=
        PmsmSimUt_P.DiscreteTimeIntegrator_LowerS_a) {
      PmsmSimUt_DW.DiscreteTimeIntegrator_DSTATE_g =
        PmsmSimUt_P.DiscreteTimeIntegrator_LowerS_a;
    }
  }

  // End of Update for DiscreteIntegrator: '<S15>/Discrete-Time Integrator'
}

// Model initialize function
void PmsmSimUtModelClass::initialize()
{
  // Registration code

  // initialize non-finites
  rt_InitInfAndNaN(sizeof(real_T));

  // InitializeConditions for DiscreteIntegrator: '<S18>/Discrete-Time Integrator' 
  PmsmSimUt_DW.DiscreteTimeIntegrator_DSTATE =
    PmsmSimUt_P.DiscreteTimeIntegrator_IC;

  // InitializeConditions for DiscreteIntegrator: '<S17>/Discrete-Time Integrator1' 
  PmsmSimUt_DW.DiscreteTimeIntegrator1_DSTATE =
    PmsmSimUt_P.DiscreteTimeIntegrator1_IC_a;

  // InitializeConditions for DiscreteIntegrator: '<S17>/Discrete-Time Integrator' 
  PmsmSimUt_DW.DiscreteTimeIntegrator_DSTATE_c = PmsmSimUt_P.Params.Flux;

  // InitializeConditions for DiscreteIntegrator: '<S16>/Discrete-Time Integrator' 
  PmsmSimUt_DW.DiscreteTimeIntegrator_DSTATE_j = PmsmSimUt_P.Subsystem1_Init;

  // InitializeConditions for Memory: '<S7>/Memory'
  PmsmSimUt_DW.Memory_PreviousInput = PmsmSimUt_P.Memory_InitialCondition;

  // InitializeConditions for DiscreteIntegrator: '<S15>/Discrete-Time Integrator' 
  PmsmSimUt_DW.DiscreteTimeIntegrator_DSTATE_g = PmsmSimUt_P.Subsystem_Init_c;

  // SystemInitialize for Enabled SubSystem: '<S18>/Subsystem'
  // InitializeConditions for DiscreteIntegrator: '<S19>/Discrete-Time Integrator1' 
  PmsmSimUt_DW.DiscreteTimeIntegrator1_DSTAT_d =
    PmsmSimUt_P.DiscreteTimeIntegrator1_IC;

  // SystemInitialize for DiscreteIntegrator: '<S19>/Discrete-Time Integrator1' incorporates:
  //   Outport: '<S19>/Wm'

  PmsmSimUt_B.DiscreteTimeIntegrator1 = PmsmSimUt_P.Wm_Y0;

  // End of SystemInitialize for SubSystem: '<S18>/Subsystem'

  // SystemInitialize for Enabled SubSystem: '<S1>/Subsystem1'
  // SystemInitialize for Fcn: '<S14>/Fcn' incorporates:
  //   Outport: '<S14>/dq'

  PmsmSimUt_B.Fcn_g = PmsmSimUt_P.dq_Y0_e[0];

  // SystemInitialize for Fcn: '<S14>/Fcn1' incorporates:
  //   Outport: '<S14>/dq'

  PmsmSimUt_B.Fcn1_e = PmsmSimUt_P.dq_Y0_e[1];

  // End of SystemInitialize for SubSystem: '<S1>/Subsystem1'

  // SystemInitialize for Enabled SubSystem: '<S1>/Subsystem - pi//2 delay'
  // SystemInitialize for Fcn: '<S13>/Fcn' incorporates:
  //   Outport: '<S13>/dq'

  PmsmSimUt_B.Fcn_b = PmsmSimUt_P.dq_Y0[0];

  // SystemInitialize for Fcn: '<S13>/Fcn1' incorporates:
  //   Outport: '<S13>/dq'

  PmsmSimUt_B.Fcn1_n = PmsmSimUt_P.dq_Y0[1];

  // End of SystemInitialize for SubSystem: '<S1>/Subsystem - pi//2 delay'

  // SystemInitialize for Enabled SubSystem: '<Root>/Subsystem'
  // InitializeConditions for DiscreteIntegrator: '<S20>/Discrete-Time Integrator' 
  PmsmSimUt_DW.DiscreteTimeIntegrator_DSTATE_n = PmsmSimUt_P.Subsystem_Init;

  // SystemInitialize for Sum: '<S20>/Sum6' incorporates:
  //   Outport: '<S5>/Iq_ref'
  //   Saturate: '<S20>/Saturation2'

  PmsmSimUt_B.Iq_ref = PmsmSimUt_P.Iq_ref_Y0;

  // End of SystemInitialize for SubSystem: '<Root>/Subsystem'

  // SystemInitialize for Enabled SubSystem: '<S21>/Subsystem1'
  // SystemInitialize for Fcn: '<S26>/Fcn' incorporates:
  //   Outport: '<S26>/dq'

  PmsmSimUt_B.Fcn = PmsmSimUt_P.dq_Y0_f[0];

  // SystemInitialize for Fcn: '<S26>/Fcn1' incorporates:
  //   Outport: '<S26>/dq'

  PmsmSimUt_B.Fcn1 = PmsmSimUt_P.dq_Y0_f[1];

  // End of SystemInitialize for SubSystem: '<S21>/Subsystem1'

  // SystemInitialize for Enabled SubSystem: '<S21>/Subsystem - pi//2 delay'
  // SystemInitialize for Fcn: '<S25>/Fcn' incorporates:
  //   Outport: '<S25>/dq'

  PmsmSimUt_B.Fcn_a = PmsmSimUt_P.dq_Y0_l[0];

  // SystemInitialize for Fcn: '<S25>/Fcn1' incorporates:
  //   Outport: '<S25>/dq'

  PmsmSimUt_B.Fcn1_p = PmsmSimUt_P.dq_Y0_l[1];

  // End of SystemInitialize for SubSystem: '<S21>/Subsystem - pi//2 delay'
}

// Model terminate function
void PmsmSimUtModelClass::terminate()
{
  // (no terminate code required)
}

// Constructor
PmsmSimUtModelClass::PmsmSimUtModelClass() :
  PmsmSimUt_U(),
  PmsmSimUt_Y(),
  PmsmSimUt_B(),
  PmsmSimUt_DW(),
  PmsmSimUt_M()
{
  // Currently there is no constructor body generated.
}

// Destructor
PmsmSimUtModelClass::~PmsmSimUtModelClass()
{
  // Currently there is no destructor body generated.
}

// Real-Time Model get method
PmsmSimUtModelClass::RT_MODEL_PmsmSimUt_T * PmsmSimUtModelClass::getRTM()
{
  return (&PmsmSimUt_M);
}

//
// File trailer for generated code.
//
// [EOF]
//