rtwdemo_pmsmfoc.cpp

00001 /*
00002  * File: rtwdemo_pmsmfoc.c
00003  *
00004  * Code generated for Simulink model 'rtwdemo_pmsmfoc'.
00005  *
00006  * Model version                  : 1.2949
00007  * Simulink Coder version         : 8.7 (R2014b) 11-Aug-2014
00008  * C/C++ source code generated on : Sat Oct 11 00:06:05 2014
00009  *
00010  * Target selection: ert.tlc
00011  * Embedded hardware selection: Generic->32-bit Embedded Processor
00012  * Code generation objective: Execution efficiency
00013  * Validation result: Not run
00014  */
00015 
00016 #include "rtwdemo_pmsmfoc.h"
00017 #include "arm_math.h"
00018 
00019 /* Named constants for Chart: '<S1>/Controller_Mode_Scheduler' */
00020 #define IN_Motor_Control               ((uint8_T)1U)
00021 #define IN_Motor_On                    ((uint8_T)1U)
00022 #define IN_NO_ACTIVE_CHILD             ((uint8_T)0U)
00023 #define IN_Position_Control            ((uint8_T)1U)
00024 #define IN_Ramp_To_Stop                ((uint8_T)2U)
00025 #define IN_Stand_By                    ((uint8_T)2U)
00026 #define IN_Startup_Open_Loop_Control   ((uint8_T)2U)
00027 #define IN_Torque_Control              ((uint8_T)3U)
00028 #define IN_Velocity_Control            ((uint8_T)4U)
00029 
00030 /* Named constants for Chart: '<S49>/Wait_For_Valid_Position' */
00031 #define IN_Position_Valid              ((uint8_T)1U)
00032 #define IN_Wait_For_Valid_Position     ((uint8_T)2U)
00033 #define IN_Wait_For_Valid_Velocity     ((uint8_T)3U)
00034 #define WaitForValidVelocityTicks      (124)
00035 
00036 /* Exported block signals */
00037 real32_T phase_currents[2];            /* '<S48>/Product' */
00038 real32_T rotor_position;               /* '<S49>/Add' */
00039 real32_T velocity_measured;            /* '<S55>/Scale_Output' */
00040 real32_T d_current_error;              /* '<S12>/Sum2' */
00041 real32_T q_current_command;            /* '<S5>/Lo_to_Hi_Rate_Transition1' */
00042 real32_T q_current_measured;           /* '<S22>/Add' */
00043 real32_T q_current_error;              /* '<S12>/Sum' */
00044 real32_T phase_voltages[3];            /* '<S27>/Select_Sector' */
00045 real32_T velocity_error;               /* '<S11>/Sum2' */
00046 EnumControllerMode controller_mode;    /* '<S1>/Controller_Mode_Scheduler' */
00047 
00048 /* Exported block parameters */
00049 CTRLPARAMS_STRUCT ctrlParams = {
00050   10.0F,
00051   10000.0F,
00052   0.005F,
00053   0.015F,
00054   0.1F,
00055   0.6F,
00056   1.0F,
00057   0.2F,
00058   20.0F,
00059   2252.25F,
00060   0.00488400506F,
00061   -0.0F,
00062   4.0F
00063 } ;                                    /* Variable: ctrlParams
00064                                         * Referenced by:
00065                                         *   '<S1>/Controller_Mode_Scheduler'
00066                                         *   '<S48>/ADC_Driver_Units_To_Amps'
00067                                         *   '<S48>/ADC_Zero_Offset'
00068                                         *   '<S49>/Offset_Between_Encoder_Zero_And_Mechanical_Zero'
00069                                         *   '<S14>/number_of_pole_pairs'
00070                                         *   '<S42>/Integral Gain'
00071                                         *   '<S42>/Proportional Gain'
00072                                         *   '<S45>/Integral Gain'
00073                                         *   '<S45>/Proportional Gain'
00074                                         *   '<S63>/Startup_Acceleration_Constant'
00075                                         *   '<S17>/Integral Gain'
00076                                         *   '<S17>/Proportional Gain'
00077                                         *   '<S18>/Integral Gain'
00078                                         *   '<S18>/Proportional Gain'
00079                                         */
00080 
00081 /* Constant parameters (auto storage) */
00082 const ConstParam ConstP = {
00083   /* Computed Parameter: Lookup_Table_table
00084    * Referenced by: '<S28>/Lookup_Table'
00085    */
00086   { 2U, 2U, 6U, 1U, 4U, 3U, 5U }
00087 };
00088 
00089 /* Block signals and states (auto storage) */
00090 D_Work DWork;
00091 
00092 /* Real-time model */
00093 RT_MODEL M_;
00094 RT_MODEL *const M = &M_;
00095 extern real32_T rt_roundf(real32_T u);
00096 extern real32_T rt_modf(real32_T u0, real32_T u1);
00097 static void rate_scheduler(void);
00098 
00099 /*
00100  *   This function updates active task flag for each subrate.
00101  * The function is called at model base rate, hence the
00102  * generated code self-manages all its subrates.
00103  */
00104 static void rate_scheduler(void)
00105 {
00106   /* Compute which subrates run during the next base time step.  Subrates
00107    * are an integer multiple of the base rate counter.  Therefore, the subtask
00108    * counter is reset when it reaches its limit (zero means run).
00109    */
00110   (M->Timing.TaskCounters.TID[1])++;
00111   if ((M->Timing.TaskCounters.TID[1]) > 124) {/* Sample time: [0.005s, 0.0s] */
00112     M->Timing.TaskCounters.TID[1] = 0;
00113   }
00114 }
00115 
00116 real32_T rt_roundf(real32_T u)
00117 {
00118   real32_T y;
00119   if (((real32_T)fabs(u)) < 8.388608E+6F) {
00120     if (u >= 0.5F) {
00121       y = (real32_T)floor(u + 0.5F);
00122     } else if (u > -0.5F) {
00123       y = 0.0F;
00124     } else {
00125       y = (real32_T)ceil(u - 0.5F);
00126     }
00127   } else {
00128     y = u;
00129   }
00130 
00131   return y;
00132 }
00133 
00134 real32_T rt_modf(real32_T u0, real32_T u1)
00135 {
00136   real32_T y;
00137   real32_T tmp;
00138   if (u1 == 0.0F) {
00139     y = u0;
00140   } else {
00141     tmp = u0 / u1;
00142     if (u1 <= ((real32_T)floor(u1))) {
00143       y = u0 - (((real32_T)floor(tmp)) * u1);
00144     } else if (((real32_T)fabs(tmp - rt_roundf(tmp))) <= (FLT_EPSILON *
00145                 ((real32_T)fabs(tmp)))) {
00146       y = 0.0F;
00147     } else {
00148       y = (tmp - ((real32_T)floor(tmp))) * u1;
00149     }
00150   }
00151 
00152   return y;
00153 }
00154 
00155 /* Model step function */
00156 extern "C" EnumErrorType Controller(uint16_T motor_on, EnumCommandType command_type,
00157   real32_T current_request, SENSORS_STRUCT *sensors, uint16_T pwm_compare[3])
00158 {
00159   real32_T sin_coefficient;
00160   int32_T Wrap_To_Pi;
00161   real32_T electrical_angle;
00162   real32_T cos_coefficient;
00163   real32_T SignDeltaU_b;
00164   int16_T Enum_To_Int;
00165   uint8_T FixPtRelationalOperator;
00166   real32_T SignDeltaU;
00167   real32_T Gain1;
00168   int8_T rtPrevAction;
00169   real32_T alpha_voltage;
00170   real32_T SignPreIntegrator_f;
00171   real32_T IntegralGain_j;
00172   boolean_T RelationalOperator;
00173   boolean_T NotEqual_b;
00174   real32_T Switch_fr;
00175   real32_T Sectors_2_and_5_idx_1;
00176   real32_T Sectors_2_and_5_idx_2;
00177   int16_T u0;
00178 
00179   /* specified return value */
00180   EnumErrorType error;
00181 
00182   /* Product: '<S48>/Product' incorporates:
00183    *  Constant: '<S48>/ADC_Driver_Units_To_Amps'
00184    *  Constant: '<S48>/ADC_Zero_Offset'
00185    *  Inport: '<Root>/sensors'
00186    *  Sum: '<S48>/Add'
00187    */
00188   phase_currents[0] = (((real32_T)sensors->adc_phase_currents[0]) -
00189                        ctrlParams.AdcZeroOffsetDriverUnits) *
00190     ctrlParams.AdcDriverUnitsToAmps;
00191   phase_currents[1] = (((real32_T)sensors->adc_phase_currents[1]) -
00192                        ctrlParams.AdcZeroOffsetDriverUnits) *
00193     ctrlParams.AdcDriverUnitsToAmps;
00194 
00195   /* Chart: '<S49>/Wait_For_Valid_Position' incorporates:
00196    *  Inport: '<Root>/sensors'
00197    */
00198   /* Gateway: Motor_Control/Sensors_To_Engineering_Units/Encoder_To_Position/Wait_For_Valid_Position */
00199   if (DWork.temporalCounter_i1 < 127U) {
00200     DWork.temporalCounter_i1++;
00201   }
00202 
00203   /* During: Motor_Control/Sensors_To_Engineering_Units/Encoder_To_Position/Wait_For_Valid_Position */
00204   if (DWork.is_active_c2_rtwdemo_pmsmfoc == 0U) {
00205     /* Entry: Motor_Control/Sensors_To_Engineering_Units/Encoder_To_Position/Wait_For_Valid_Position */
00206     DWork.is_active_c2_rtwdemo_pmsmfoc = 1U;
00207 
00208     /* Entry Internal: Motor_Control/Sensors_To_Engineering_Units/Encoder_To_Position/Wait_For_Valid_Position */
00209     /* Transition: '<S52>:113' */
00210     if (sensors->encoder_valid == 0) {
00211       /* Transition: '<S52>:114' */
00212       /* Transition: '<S52>:115' */
00213       DWork.is_c2_rtwdemo_pmsmfoc = IN_Wait_For_Valid_Position;
00214     } else {
00215       /* Transition: '<S52>:120' */
00216       /* Transition: '<S52>:119' */
00217       DWork.is_c2_rtwdemo_pmsmfoc = IN_Position_Valid;
00218       DWork.Position_Valid = 1U;
00219     }
00220   } else {
00221     switch (DWork.is_c2_rtwdemo_pmsmfoc) {
00222      case IN_Position_Valid:
00223       /* During 'Position_Valid': '<S52>:25' */
00224       break;
00225 
00226      case IN_Wait_For_Valid_Position:
00227       /* During 'Wait_For_Valid_Position': '<S52>:99' */
00228       if (sensors->encoder_valid != 0) {
00229         /* Transition: '<S52>:100' */
00230         DWork.is_c2_rtwdemo_pmsmfoc = IN_Wait_For_Valid_Velocity;
00231         DWork.temporalCounter_i1 = 0U;
00232       }
00233       break;
00234 
00235      default:
00236       /* During 'Wait_For_Valid_Velocity': '<S52>:101' */
00237       if (DWork.temporalCounter_i1 >= WaitForValidVelocityTicks) {
00238         /* Transition: '<S52>:102' */
00239         DWork.is_c2_rtwdemo_pmsmfoc = IN_Position_Valid;
00240         DWork.Position_Valid = 1U;
00241       }
00242       break;
00243     }
00244   }
00245 
00246   /* End of Chart: '<S49>/Wait_For_Valid_Position' */
00247 
00248   /* Sum: '<S49>/Add' incorporates:
00249    *  Constant: '<S49>/Offset_Between_Encoder_Zero_And_Mechanical_Zero'
00250    *  Gain: '<S49>/radians_per_counts'
00251    *  Inport: '<Root>/sensors'
00252    */
00253   rotor_position = (0.000785398181F * ((real32_T)sensors->encoder_counter)) +
00254     ctrlParams.EncoderToMechanicalZeroOffsetRads;
00255   if (M->Timing.TaskCounters.TID[1] == 0) {
00256     /* Gain: '<S55>/Wrap_To_Pi' incorporates:
00257      *  DataTypeConversion: '<S55>/Convert_to_Uint32'
00258      *  Gain: '<S55>/Scale_Input'
00259      */
00260     Wrap_To_Pi = (((int32_T)(1.70891312E+8F * rotor_position)) << 2);
00261 
00262     /* Gain: '<S55>/Scale_Output' incorporates:
00263      *  DataTypeConversion: '<S55>/Difference_to_Single'
00264      *  Delay: '<S55>/Position_Delay'
00265      *  Sum: '<S55>/Difference_Wrap'
00266      */
00267     velocity_measured = ((real32_T)(Wrap_To_Pi - DWork.Position_Delay_DSTATE)) *
00268       2.92583621E-7F;
00269 
00270     /* Chart: '<S1>/Controller_Mode_Scheduler' incorporates:
00271      *  Inport: '<Root>/command_type'
00272      *  Inport: '<Root>/command_value'
00273      *  Inport: '<Root>/motor_on'
00274      */
00275     /* Gateway: Mode_Scheduler/Controller_Mode_Scheduler */
00276     /* During: Mode_Scheduler/Controller_Mode_Scheduler */
00277     if (DWork.is_c1_rtwdemo_pmsmfoc == IN_Motor_On) {
00278       /* During 'Motor_On': '<S4>:338' */
00279       if (DWork.error_l != 0) {
00280         /* Transition: '<S4>:339' */
00281         /* Transition: '<S4>:353' */
00282         /* Exit Internal 'Motor_On': '<S4>:338' */
00283         /* Exit Internal 'Motor_Control': '<S4>:344' */
00284         DWork.is_Motor_Control = IN_NO_ACTIVE_CHILD;
00285         DWork.is_Motor_On = IN_NO_ACTIVE_CHILD;
00286         DWork.is_c1_rtwdemo_pmsmfoc = IN_Stand_By;
00287 
00288         /* Entry 'Stand_By': '<S4>:154' */
00289         controller_mode = StandBy;
00290       } else if (DWork.is_Motor_On == IN_Motor_Control) {
00291         /* During 'Motor_Control': '<S4>:344' */
00292         if (!(motor_on != 0)) {
00293           /* Transition: '<S4>:282' */
00294           /* Exit Internal 'Motor_Control': '<S4>:344' */
00295           DWork.is_Motor_Control = IN_NO_ACTIVE_CHILD;
00296           DWork.is_Motor_On = IN_Ramp_To_Stop;
00297 
00298           /* Entry 'Ramp_To_Stop': '<S4>:270' */
00299           controller_mode = VelocityControl;
00300           DWork.velocity_command = 0.0F;
00301           DWork.torque_command = 0.0F;
00302         } else {
00303           switch (DWork.is_Motor_Control) {
00304            case IN_Position_Control:
00305             /* During 'Position_Control': '<S4>:226' */
00306             DWork.position_command = current_request;
00307             break;
00308 
00309            case IN_Startup_Open_Loop_Control:
00310             /* During 'Startup_Open_Loop_Control': '<S4>:103' */
00311             if (DWork.Position_Valid != 0) {
00312               /* Transition: '<S4>:157' */
00313               /* Transition: '<S4>:233' */
00314               switch (command_type) {
00315                case Velocity:
00316                 /* Transition: '<S4>:162' */
00317                 DWork.is_Motor_Control = IN_Velocity_Control;
00318 
00319                 /* Entry 'Velocity_Control': '<S4>:108' */
00320                 controller_mode = VelocityControl;
00321                 DWork.velocity_command = current_request;
00322                 break;
00323 
00324                case Position:
00325                 /* Transition: '<S4>:235' */
00326                 /* Transition: '<S4>:234' */
00327                 DWork.is_Motor_Control = IN_Position_Control;
00328 
00329                 /* Entry 'Position_Control': '<S4>:226' */
00330                 controller_mode = PositionControl;
00331                 DWork.position_command = current_request;
00332                 break;
00333 
00334                default:
00335                 /* Transition: '<S4>:237' */
00336                 /* Transition: '<S4>:158' */
00337                 /*  [command_type==Torque] */
00338                 DWork.is_Motor_Control = IN_Torque_Control;
00339 
00340                 /* Entry 'Torque_Control': '<S4>:220' */
00341                 controller_mode = TorqueControl;
00342                 DWork.torque_command = current_request;
00343                 break;
00344               }
00345             }
00346             break;
00347 
00348            case IN_Torque_Control:
00349             /* During 'Torque_Control': '<S4>:220' */
00350             DWork.torque_command = current_request;
00351             break;
00352 
00353            default:
00354             /* During 'Velocity_Control': '<S4>:108' */
00355             DWork.velocity_command = current_request;
00356             break;
00357           }
00358         }
00359       } else {
00360         /* During 'Ramp_To_Stop': '<S4>:270' */
00361         if (((real32_T)fabs(velocity_measured)) < ctrlParams.RampToStopVelocity)
00362         {
00363           /* Transition: '<S4>:169' */
00364           /* Transition: '<S4>:355' */
00365           DWork.is_Motor_On = IN_NO_ACTIVE_CHILD;
00366           DWork.is_c1_rtwdemo_pmsmfoc = IN_Stand_By;
00367 
00368           /* Entry 'Stand_By': '<S4>:154' */
00369           controller_mode = StandBy;
00370         }
00371       }
00372     } else {
00373       /* During 'Stand_By': '<S4>:154' */
00374       if ((motor_on != 0) && (!(DWork.error_l != 0))) {
00375         /* Transition: '<S4>:164' */
00376         if (!(DWork.Position_Valid != 0)) {
00377           /* Transition: '<S4>:133' */
00378           DWork.is_c1_rtwdemo_pmsmfoc = IN_Motor_On;
00379           DWork.is_Motor_On = IN_Motor_Control;
00380           DWork.is_Motor_Control = IN_Startup_Open_Loop_Control;
00381 
00382           /* Entry 'Startup_Open_Loop_Control': '<S4>:103' */
00383           controller_mode = Startup;
00384           DWork.torque_command = ctrlParams.StartupCurrent;
00385         } else {
00386           /* Transition: '<S4>:137' */
00387           /* Transition: '<S4>:233' */
00388           switch (command_type) {
00389            case Velocity:
00390             /* Transition: '<S4>:162' */
00391             DWork.is_c1_rtwdemo_pmsmfoc = IN_Motor_On;
00392             DWork.is_Motor_On = IN_Motor_Control;
00393             DWork.is_Motor_Control = IN_Velocity_Control;
00394 
00395             /* Entry 'Velocity_Control': '<S4>:108' */
00396             controller_mode = VelocityControl;
00397             DWork.velocity_command = current_request;
00398             break;
00399 
00400            case Position:
00401             /* Transition: '<S4>:235' */
00402             /* Transition: '<S4>:234' */
00403             DWork.is_c1_rtwdemo_pmsmfoc = IN_Motor_On;
00404             DWork.is_Motor_On = IN_Motor_Control;
00405             DWork.is_Motor_Control = IN_Position_Control;
00406 
00407             /* Entry 'Position_Control': '<S4>:226' */
00408             controller_mode = PositionControl;
00409             DWork.position_command = current_request;
00410             break;
00411 
00412            default:
00413             /* Transition: '<S4>:237' */
00414             /* Transition: '<S4>:158' */
00415             /*  [command_type==Torque] */
00416             DWork.is_c1_rtwdemo_pmsmfoc = IN_Motor_On;
00417             DWork.is_Motor_On = IN_Motor_Control;
00418             DWork.is_Motor_Control = IN_Torque_Control;
00419 
00420             /* Entry 'Torque_Control': '<S4>:220' */
00421             controller_mode = TorqueControl;
00422             DWork.torque_command = current_request;
00423             break;
00424           }
00425         }
00426       }
00427     }
00428 
00429     /* End of Chart: '<S1>/Controller_Mode_Scheduler' */
00430 
00431     /* RelationalOperator: '<S51>/Relational Operator' incorporates:
00432      *  Constant: '<S62>/Constant'
00433      */
00434     DWork.RelationalOperator_a = (controller_mode == Startup);
00435   }
00436 
00437   /* Outputs for Enabled SubSystem: '<S51>/Open Loop Position' incorporates:
00438    *  EnablePort: '<S63>/Enable'
00439    */
00440   if (DWork.RelationalOperator_a) {
00441     if (!DWork.OpenLoopPosition_MODE) {
00442       /* InitializeConditions for DiscreteIntegrator: '<S63>/Integrate_To_Position' */
00443       DWork.Integrate_To_Position_DSTATE = 0.0F;
00444 
00445       /* InitializeConditions for DiscreteIntegrator: '<S63>/Integrate_To_Velocity' */
00446       DWork.Integrate_To_Velocity_DSTATE = 0.0F;
00447       DWork.OpenLoopPosition_MODE = true;
00448     }
00449 
00450     /* DiscreteIntegrator: '<S63>/Integrate_To_Position' */
00451     DWork.position = DWork.Integrate_To_Position_DSTATE;
00452 
00453     /* Update for DiscreteIntegrator: '<S63>/Integrate_To_Position' incorporates:
00454      *  DiscreteIntegrator: '<S63>/Integrate_To_Velocity'
00455      */
00456     DWork.Integrate_To_Position_DSTATE += 4.0E-5F *
00457       DWork.Integrate_To_Velocity_DSTATE;
00458 
00459     /* Update for DiscreteIntegrator: '<S63>/Integrate_To_Velocity' incorporates:
00460      *  Constant: '<S63>/Startup_Acceleration_Constant'
00461      */
00462     DWork.Integrate_To_Velocity_DSTATE += 4.0E-5F *
00463       ctrlParams.StartupAcceleration;
00464   } else {
00465     if (DWork.OpenLoopPosition_MODE) {
00466       DWork.OpenLoopPosition_MODE = false;
00467     }
00468   }
00469 
00470   /* End of Outputs for SubSystem: '<S51>/Open Loop Position' */
00471 
00472   /* Switch: '<S51>/Switch' */
00473   if (DWork.RelationalOperator_a) {
00474     Switch_fr = DWork.position;
00475   } else {
00476     Switch_fr = rotor_position;
00477   }
00478 
00479   /* End of Switch: '<S51>/Switch' */
00480 
00481   /* Gain: '<S14>/number_of_pole_pairs' */
00482   electrical_angle = ctrlParams.PmsmPolePairs * Switch_fr;
00483 
00484   /* Trigonometry: '<S14>/sine_cosine' */
00485   //sin_coefficient = (real32_T)sin(electrical_angle);
00486   //cos_coefficient = (real32_T)cos(electrical_angle);
00487   sin_coefficient = arm_sin_f32(electrical_angle);
00488   cos_coefficient = arm_cos_f32(electrical_angle);
00489   
00490   /* Gain: '<S21>/Beta_Gain' incorporates:
00491    *  Gain: '<S21>/B_Gain'
00492    *  Sum: '<S21>/Add'
00493    */
00494   SignDeltaU_b = ((2.0F * phase_currents[1]) + phase_currents[0]) * 0.577350259F;
00495 
00496   /* Sum: '<S12>/Sum2' incorporates:
00497    *  Constant: '<S12>/d_current_command (A)'
00498    *  Product: '<S22>/Product2'
00499    *  Product: '<S22>/Product3'
00500    *  Sum: '<S22>/Add1'
00501    */
00502   d_current_error = 0.0F - ((phase_currents[0] * cos_coefficient) +
00503     (SignDeltaU_b * sin_coefficient));
00504 
00505   /* DataTypeConversion: '<S8>/Enum_To_Int' */
00506   Enum_To_Int = (int16_T)controller_mode;
00507 
00508   /* RelationalOperator: '<S13>/FixPt Relational Operator' incorporates:
00509    *  UnitDelay: '<S13>/Delay Input1'
00510    */
00511   FixPtRelationalOperator = (uint8_T)(Enum_To_Int != DWork.DelayInput1_DSTATE);
00512 
00513   /* DiscreteIntegrator: '<S17>/Integrator' */
00514   if ((FixPtRelationalOperator != 0) || (DWork.Integrator_PrevResetState != 0))
00515   {
00516     DWork.Integrator_DSTATE = 0.0F;
00517   }
00518 
00519   /* Sum: '<S17>/Sum' incorporates:
00520    *  DiscreteIntegrator: '<S17>/Integrator'
00521    *  Gain: '<S17>/Proportional Gain'
00522    */
00523   SignDeltaU = (ctrlParams.Current_P * d_current_error) +
00524     DWork.Integrator_DSTATE;
00525 
00526   /* Saturate: '<S17>/Saturate' */
00527   if (SignDeltaU > 12.0F) {
00528     Gain1 = 12.0F;
00529   } else if (SignDeltaU < -12.0F) {
00530     Gain1 = -12.0F;
00531   } else {
00532     Gain1 = SignDeltaU;
00533   }
00534 
00535   /* End of Saturate: '<S17>/Saturate' */
00536   if (M->Timing.TaskCounters.TID[1] == 0) {
00537     /* SwitchCase: '<S5>/Switch Case' incorporates:
00538      *  Inport: '<S10>/torque_command'
00539      */
00540     rtPrevAction = DWork.SwitchCase_ActiveSubsystem;
00541     switch (controller_mode) {
00542      case VelocityControl:
00543       DWork.SwitchCase_ActiveSubsystem = 0;
00544       break;
00545 
00546      case PositionControl:
00547       DWork.SwitchCase_ActiveSubsystem = 1;
00548       break;
00549 
00550      default:
00551       DWork.SwitchCase_ActiveSubsystem = 2;
00552       break;
00553     }
00554 
00555     switch (DWork.SwitchCase_ActiveSubsystem) {
00556      case 0:
00557       if (DWork.SwitchCase_ActiveSubsystem != rtPrevAction) {
00558         /* InitializeConditions for IfAction SubSystem: '<S5>/Velocity_Control' incorporates:
00559          *  InitializeConditions for ActionPort: '<S11>/Action Port'
00560          */
00561         /* InitializeConditions for SwitchCase: '<S5>/Switch Case' incorporates:
00562          *  InitializeConditions for DiscreteIntegrator: '<S45>/Integrator'
00563          */
00564         DWork.Integrator_DSTATE_f = 0.0F;
00565 
00566         /* End of InitializeConditions for SubSystem: '<S5>/Velocity_Control' */
00567       }
00568 
00569       /* Outputs for IfAction SubSystem: '<S5>/Velocity_Control' incorporates:
00570        *  ActionPort: '<S11>/Action Port'
00571        */
00572       /* Sum: '<S11>/Sum2' */
00573       velocity_error = DWork.velocity_command - velocity_measured;
00574 
00575       /* Sum: '<S45>/Sum' incorporates:
00576        *  DiscreteIntegrator: '<S45>/Integrator'
00577        *  Gain: '<S45>/Proportional Gain'
00578        */
00579       electrical_angle = (ctrlParams.Velocity_P * velocity_error) +
00580         DWork.Integrator_DSTATE_f;
00581 
00582       /* Saturate: '<S45>/Saturate' */
00583       if (electrical_angle > 2.0F) {
00584         /* SignalConversion: '<S11>/Isolate_For_Merge' */
00585         DWork.Merge = 2.0F;
00586       } else if (electrical_angle < -2.0F) {
00587         /* SignalConversion: '<S11>/Isolate_For_Merge' */
00588         DWork.Merge = -2.0F;
00589       } else {
00590         /* SignalConversion: '<S11>/Isolate_For_Merge' */
00591         DWork.Merge = electrical_angle;
00592       }
00593 
00594       /* End of Saturate: '<S45>/Saturate' */
00595 
00596       /* DeadZone: '<S46>/DeadZone' */
00597       if (electrical_angle > 2.0F) {
00598         electrical_angle -= 2.0F;
00599       } else if (electrical_angle >= -2.0F) {
00600         electrical_angle = 0.0F;
00601       } else {
00602         electrical_angle -= -2.0F;
00603       }
00604 
00605       /* End of DeadZone: '<S46>/DeadZone' */
00606 
00607       /* RelationalOperator: '<S46>/NotEqual' */
00608       NotEqual_b = (0.0F != electrical_angle);
00609 
00610       /* Signum: '<S46>/SignDeltaU' */
00611       if (electrical_angle < 0.0F) {
00612         electrical_angle = -1.0F;
00613       } else {
00614         if (electrical_angle > 0.0F) {
00615           electrical_angle = 1.0F;
00616         }
00617       }
00618 
00619       /* End of Signum: '<S46>/SignDeltaU' */
00620 
00621       /* Gain: '<S45>/Integral Gain' */
00622       Switch_fr = ctrlParams.Velocity_I * velocity_error;
00623 
00624       /* DataTypeConversion: '<S46>/DataTypeConv1' */
00625       if (electrical_angle < 128.0F) {
00626         rtPrevAction = (int8_T)electrical_angle;
00627       } else {
00628         rtPrevAction = MAX_int8_T;
00629       }
00630 
00631       /* End of DataTypeConversion: '<S46>/DataTypeConv1' */
00632 
00633       /* Signum: '<S46>/SignPreIntegrator' */
00634       if (Switch_fr < 0.0F) {
00635         electrical_angle = -1.0F;
00636       } else if (Switch_fr > 0.0F) {
00637         electrical_angle = 1.0F;
00638       } else {
00639         electrical_angle = Switch_fr;
00640       }
00641 
00642       /* Switch: '<S45>/Switch' incorporates:
00643        *  Constant: '<S45>/Constant'
00644        *  DataTypeConversion: '<S46>/DataTypeConv2'
00645        *  Logic: '<S46>/AND'
00646        *  RelationalOperator: '<S46>/Equal'
00647        *  Signum: '<S46>/SignPreIntegrator'
00648        */
00649       if (NotEqual_b && (rtPrevAction == ((int8_T)electrical_angle))) {
00650         Switch_fr = 0.0F;
00651       }
00652 
00653       /* End of Switch: '<S45>/Switch' */
00654 
00655       /* Update for DiscreteIntegrator: '<S45>/Integrator' */
00656       DWork.Integrator_DSTATE_f += 0.005F * Switch_fr;
00657 
00658       /* End of Outputs for SubSystem: '<S5>/Velocity_Control' */
00659       break;
00660 
00661      case 1:
00662       if (DWork.SwitchCase_ActiveSubsystem != rtPrevAction) {
00663         /* InitializeConditions for IfAction SubSystem: '<S5>/Position_Control' incorporates:
00664          *  InitializeConditions for ActionPort: '<S9>/Action Port'
00665          */
00666         /* InitializeConditions for SwitchCase: '<S5>/Switch Case' incorporates:
00667          *  InitializeConditions for DiscreteIntegrator: '<S42>/Integrator'
00668          */
00669         DWork.Integrator_DSTATE_lc = 0.0F;
00670 
00671         /* End of InitializeConditions for SubSystem: '<S5>/Position_Control' */
00672       }
00673 
00674       /* Outputs for IfAction SubSystem: '<S5>/Position_Control' incorporates:
00675        *  ActionPort: '<S9>/Action Port'
00676        */
00677       /* Sum: '<S9>/Sum2' */
00678       Switch_fr = DWork.position_command - Switch_fr;
00679 
00680       /* Switch: '<S43>/Select_Angle' incorporates:
00681        *  Constant: '<S43>/Neg_Pi_Constant'
00682        *  Constant: '<S43>/Pi_Constant_1'
00683        *  Constant: '<S43>/Pi_Constant_2'
00684        *  Constant: '<S43>/Pi_Constant_3'
00685        *  Constant: '<S43>/Two_Pi_Constant'
00686        *  Logic: '<S43>/OR'
00687        *  Math: '<S43>/Modulus'
00688        *  RelationalOperator: '<S43>/Greater_Than'
00689        *  RelationalOperator: '<S43>/Less_Than'
00690        *  Sum: '<S43>/Add'
00691        *  Sum: '<S43>/Subtract'
00692        */
00693       if ((Switch_fr < -1.57079637F) || (Switch_fr >= 1.57079637F)) {
00694         Switch_fr = rt_modf(Switch_fr + 1.57079637F, 3.14159274F) - 1.57079637F;
00695       }
00696 
00697       /* End of Switch: '<S43>/Select_Angle' */
00698 
00699       /* Sum: '<S42>/Sum' incorporates:
00700        *  DiscreteIntegrator: '<S42>/Integrator'
00701        *  Gain: '<S42>/Proportional Gain'
00702        */
00703       electrical_angle = (ctrlParams.Position_P * Switch_fr) +
00704         DWork.Integrator_DSTATE_lc;
00705 
00706       /* Saturate: '<S42>/Saturate' */
00707       if (electrical_angle > 2.0F) {
00708         /* SignalConversion: '<S9>/Isolate_For_Merge' */
00709         DWork.Merge = 2.0F;
00710       } else if (electrical_angle < -2.0F) {
00711         /* SignalConversion: '<S9>/Isolate_For_Merge' */
00712         DWork.Merge = -2.0F;
00713       } else {
00714         /* SignalConversion: '<S9>/Isolate_For_Merge' */
00715         DWork.Merge = electrical_angle;
00716       }
00717 
00718       /* End of Saturate: '<S42>/Saturate' */
00719 
00720       /* DeadZone: '<S44>/DeadZone' */
00721       if (electrical_angle > 2.0F) {
00722         electrical_angle -= 2.0F;
00723       } else if (electrical_angle >= -2.0F) {
00724         electrical_angle = 0.0F;
00725       } else {
00726         electrical_angle -= -2.0F;
00727       }
00728 
00729       /* End of DeadZone: '<S44>/DeadZone' */
00730 
00731       /* RelationalOperator: '<S44>/NotEqual' */
00732       NotEqual_b = (0.0F != electrical_angle);
00733 
00734       /* Signum: '<S44>/SignDeltaU' */
00735       if (electrical_angle < 0.0F) {
00736         electrical_angle = -1.0F;
00737       } else {
00738         if (electrical_angle > 0.0F) {
00739           electrical_angle = 1.0F;
00740         }
00741       }
00742 
00743       /* End of Signum: '<S44>/SignDeltaU' */
00744 
00745       /* Gain: '<S42>/Integral Gain' */
00746       Switch_fr *= ctrlParams.Position_I;
00747 
00748       /* DataTypeConversion: '<S44>/DataTypeConv1' */
00749       if (electrical_angle < 128.0F) {
00750         rtPrevAction = (int8_T)electrical_angle;
00751       } else {
00752         rtPrevAction = MAX_int8_T;
00753       }
00754 
00755       /* End of DataTypeConversion: '<S44>/DataTypeConv1' */
00756 
00757       /* Signum: '<S44>/SignPreIntegrator' */
00758       if (Switch_fr < 0.0F) {
00759         electrical_angle = -1.0F;
00760       } else if (Switch_fr > 0.0F) {
00761         electrical_angle = 1.0F;
00762       } else {
00763         electrical_angle = Switch_fr;
00764       }
00765 
00766       /* Switch: '<S42>/Switch' incorporates:
00767        *  Constant: '<S42>/Constant'
00768        *  DataTypeConversion: '<S44>/DataTypeConv2'
00769        *  Logic: '<S44>/AND'
00770        *  RelationalOperator: '<S44>/Equal'
00771        *  Signum: '<S44>/SignPreIntegrator'
00772        */
00773       if (NotEqual_b && (rtPrevAction == ((int8_T)electrical_angle))) {
00774         Switch_fr = 0.0F;
00775       }
00776 
00777       /* End of Switch: '<S42>/Switch' */
00778 
00779       /* Update for DiscreteIntegrator: '<S42>/Integrator' */
00780       DWork.Integrator_DSTATE_lc += 0.005F * Switch_fr;
00781 
00782       /* End of Outputs for SubSystem: '<S5>/Position_Control' */
00783       break;
00784 
00785      case 2:
00786       /* Outputs for IfAction SubSystem: '<S5>/Torque_Control' incorporates:
00787        *  ActionPort: '<S10>/Action Port'
00788        */
00789       DWork.Merge = DWork.torque_command;
00790 
00791       /* End of Outputs for SubSystem: '<S5>/Torque_Control' */
00792       break;
00793     }
00794 
00795     /* End of SwitchCase: '<S5>/Switch Case' */
00796   }
00797 
00798   /* RateTransition: '<S5>/Lo_to_Hi_Rate_Transition1' */
00799   q_current_command = DWork.Merge;
00800 
00801   /* Sum: '<S22>/Add' incorporates:
00802    *  Product: '<S22>/Product'
00803    *  Product: '<S22>/Product1'
00804    */
00805   q_current_measured = (SignDeltaU_b * cos_coefficient) - (phase_currents[0] *
00806     sin_coefficient);
00807 
00808   /* Sum: '<S12>/Sum' */
00809   q_current_error = q_current_command - q_current_measured;
00810 
00811   /* DiscreteIntegrator: '<S18>/Integrator' */
00812   if ((FixPtRelationalOperator != 0) || (DWork.Integrator_PrevResetState_c != 0))
00813   {
00814     DWork.Integrator_DSTATE_l = 0.0F;
00815   }
00816 
00817   /* Sum: '<S18>/Sum' incorporates:
00818    *  DiscreteIntegrator: '<S18>/Integrator'
00819    *  Gain: '<S18>/Proportional Gain'
00820    */
00821   SignDeltaU_b = (ctrlParams.Current_P * q_current_error) +
00822     DWork.Integrator_DSTATE_l;
00823 
00824   /* Saturate: '<S18>/Saturate' */
00825   if (SignDeltaU_b > 12.0F) {
00826     alpha_voltage = 12.0F;
00827   } else if (SignDeltaU_b < -12.0F) {
00828     alpha_voltage = -12.0F;
00829   } else {
00830     alpha_voltage = SignDeltaU_b;
00831   }
00832 
00833   /* End of Saturate: '<S18>/Saturate' */
00834 
00835   /* Sum: '<S24>/Add' incorporates:
00836    *  Product: '<S24>/Product'
00837    *  Product: '<S24>/Product1'
00838    */
00839   SignPreIntegrator_f = (Gain1 * sin_coefficient) + (alpha_voltage *
00840     cos_coefficient);
00841 
00842   /* Gain: '<S29>/Gain' */
00843   IntegralGain_j = 0.5F * SignPreIntegrator_f;
00844 
00845   /* Sum: '<S24>/Add1' incorporates:
00846    *  Product: '<S24>/Product2'
00847    *  Product: '<S24>/Product3'
00848    */
00849   alpha_voltage = (Gain1 * cos_coefficient) - (alpha_voltage * sin_coefficient);
00850 
00851   /* Gain: '<S29>/Gain1' */
00852   Gain1 = 0.866025388F * alpha_voltage;
00853 
00854   /* Gain: '<S30>/Space_Vector_Gain' incorporates:
00855    *  Gain: '<S30>/Alpha_Gain'
00856    *  Sum: '<S30>/Add'
00857    */
00858   electrical_angle = (((1.73205078F * alpha_voltage) + 33.941124F) +
00859                       SignPreIntegrator_f) * 0.353553385F;
00860 
00861   /* Gain: '<S33>/Va_Gain' incorporates:
00862    *  Gain: '<S33>/Alpha_Gain'
00863    *  Gain: '<S33>/Beta_Gain'
00864    *  Sum: '<S33>/Add'
00865    */
00866   Switch_fr = ((33.941124F - (1.73205078F * alpha_voltage)) + (3.0F *
00867     SignPreIntegrator_f)) * 0.353553385F;
00868 
00869   /* Gain: '<S36>/Space_Vector_Gain' incorporates:
00870    *  Gain: '<S36>/Alpha_Gain'
00871    *  Sum: '<S36>/Add'
00872    */
00873   cos_coefficient = ((33.941124F - (1.73205078F * alpha_voltage)) -
00874                      SignPreIntegrator_f) * 0.353553385F;
00875 
00876   /* Gain: '<S31>/Space_Vector_Gain' incorporates:
00877    *  Constant: '<S27>/Bus_Voltage'
00878    *  Gain: '<S31>/Alpha_Gain'
00879    *  Sum: '<S31>/Add'
00880    */
00881   sin_coefficient = ((2.44948983F * alpha_voltage) + 24.0F) * 0.5F;
00882 
00883   /* Gain: '<S34>/Va_Gain' incorporates:
00884    *  Constant: '<S27>/Bus_Voltage'
00885    *  Gain: '<S34>/Beta_Gain'
00886    *  Sum: '<S34>/Add'
00887    */
00888   Sectors_2_and_5_idx_1 = ((1.41421354F * SignPreIntegrator_f) + 24.0F) * 0.5F;
00889 
00890   /* Gain: '<S37>/Space_Vector_Gain' incorporates:
00891    *  Constant: '<S27>/Bus_Voltage'
00892    *  Gain: '<S37>/Beta_Gain'
00893    *  Sum: '<S37>/Add'
00894    */
00895   Sectors_2_and_5_idx_2 = (24.0F - (1.41421354F * SignPreIntegrator_f)) * 0.5F;
00896 
00897   /* Gain: '<S32>/Space_Vector_Gain' incorporates:
00898    *  Gain: '<S32>/Alpha_Gain'
00899    *  Sum: '<S32>/Add'
00900    */
00901   phase_voltages[0] = (((1.73205078F * alpha_voltage) + 33.941124F) -
00902                        SignPreIntegrator_f) * 0.353553385F;
00903 
00904   /* Gain: '<S35>/Va_Gain' incorporates:
00905    *  Gain: '<S35>/Alpha_Gain'
00906    *  Sum: '<S35>/Add'
00907    */
00908   phase_voltages[1] = ((33.941124F - (1.73205078F * alpha_voltage)) +
00909                        SignPreIntegrator_f) * 0.353553385F;
00910 
00911   /* Gain: '<S38>/Space_Vector_Gain' incorporates:
00912    *  Gain: '<S38>/Alpha_Gain'
00913    *  Gain: '<S38>/Beta_Gain'
00914    *  Sum: '<S38>/Add'
00915    */
00916   phase_voltages[2] = ((33.941124F - (1.73205078F * alpha_voltage)) - (3.0F *
00917     SignPreIntegrator_f)) * 0.353553385F;
00918 
00919   /* LookupNDDirect: '<S28>/Lookup_Table' incorporates:
00920    *  Constant: '<S39>/Constant'
00921    *  Constant: '<S40>/Constant'
00922    *  Constant: '<S41>/Constant'
00923    *  Gain: '<S28>/Sector_Gain_VB'
00924    *  Gain: '<S28>/Sector_Gain_VC'
00925    *  RelationalOperator: '<S39>/Compare'
00926    *  RelationalOperator: '<S40>/Compare'
00927    *  RelationalOperator: '<S41>/Compare'
00928    *  Sum: '<S28>/Calculate_Phase_Advanced_Sector'
00929    *  Sum: '<S29>/Add'
00930    *  Sum: '<S29>/Add1'
00931    *
00932    * About '<S28>/Lookup_Table':
00933    *  1-dimensional Direct Look-Up returning a Scalar
00934    */
00935   u0 = (int16_T)(((((Gain1 - IntegralGain_j) > 0.0F) << 1) +
00936                   (SignPreIntegrator_f > 0.0F)) + ((((0.0F - IntegralGain_j) -
00937     Gain1) > 0.0F) << 2));
00938   if (u0 > 6) {
00939     u0 = 6;
00940   }
00941 
00942   /* MultiPortSwitch: '<S27>/Select_Sector' incorporates:
00943    *  LookupNDDirect: '<S28>/Lookup_Table'
00944    *
00945    * About '<S28>/Lookup_Table':
00946    *  1-dimensional Direct Look-Up returning a Scalar
00947    */
00948   switch (ConstP.Lookup_Table_table[u0]) {
00949    case 1:
00950     phase_voltages[0] = electrical_angle;
00951     phase_voltages[1] = Switch_fr;
00952     phase_voltages[2] = cos_coefficient;
00953     break;
00954 
00955    case 2:
00956     phase_voltages[0] = sin_coefficient;
00957     phase_voltages[1] = Sectors_2_and_5_idx_1;
00958     phase_voltages[2] = Sectors_2_and_5_idx_2;
00959     break;
00960 
00961    case 3:
00962     break;
00963 
00964    case 4:
00965     phase_voltages[0] = electrical_angle;
00966     phase_voltages[1] = Switch_fr;
00967     phase_voltages[2] = cos_coefficient;
00968     break;
00969 
00970    case 5:
00971     phase_voltages[0] = sin_coefficient;
00972     phase_voltages[1] = Sectors_2_and_5_idx_1;
00973     phase_voltages[2] = Sectors_2_and_5_idx_2;
00974     break;
00975   }
00976 
00977   /* End of MultiPortSwitch: '<S27>/Select_Sector' */
00978 
00979   /* Switch: '<S6>/Switch' */
00980   if (DWork.Lo_to_Hi_Rate_Transition3_Buffe) {
00981     /* Outport: '<Root>/pwm_compare' */
00982     pwm_compare[0] = 1500U;
00983     pwm_compare[1] = 1500U;
00984     pwm_compare[2] = 1500U;
00985   } else {
00986     /* Gain: '<S6>/Voltage to PWM Compare Units' */
00987     electrical_angle = 125.0F * phase_voltages[0];
00988 
00989     /* Saturate: '<S6>/Saturation' */
00990     if (electrical_angle > 2999.0F) {
00991       /* Outport: '<Root>/pwm_compare' incorporates:
00992        *  DataTypeConversion: '<S6>/Quantize'
00993        */
00994       pwm_compare[0] = 2999U;
00995     } else if (electrical_angle < 0.0F) {
00996       /* Outport: '<Root>/pwm_compare' incorporates:
00997        *  DataTypeConversion: '<S6>/Quantize'
00998        */
00999       pwm_compare[0] = 0U;
01000     } else {
01001       /* Outport: '<Root>/pwm_compare' incorporates:
01002        *  DataTypeConversion: '<S6>/Quantize'
01003        */
01004       pwm_compare[0] = (uint16_T)electrical_angle;
01005     }
01006 
01007     /* Gain: '<S6>/Voltage to PWM Compare Units' */
01008     electrical_angle = 125.0F * phase_voltages[1];
01009 
01010     /* Saturate: '<S6>/Saturation' */
01011     if (electrical_angle > 2999.0F) {
01012       /* Outport: '<Root>/pwm_compare' incorporates:
01013        *  DataTypeConversion: '<S6>/Quantize'
01014        */
01015       pwm_compare[1] = 2999U;
01016     } else if (electrical_angle < 0.0F) {
01017       /* Outport: '<Root>/pwm_compare' incorporates:
01018        *  DataTypeConversion: '<S6>/Quantize'
01019        */
01020       pwm_compare[1] = 0U;
01021     } else {
01022       /* Outport: '<Root>/pwm_compare' incorporates:
01023        *  DataTypeConversion: '<S6>/Quantize'
01024        */
01025       pwm_compare[1] = (uint16_T)electrical_angle;
01026     }
01027 
01028     /* Gain: '<S6>/Voltage to PWM Compare Units' */
01029     electrical_angle = 125.0F * phase_voltages[2];
01030 
01031     /* Saturate: '<S6>/Saturation' */
01032     if (electrical_angle > 2999.0F) {
01033       /* Outport: '<Root>/pwm_compare' incorporates:
01034        *  DataTypeConversion: '<S6>/Quantize'
01035        */
01036       pwm_compare[2] = 2999U;
01037     } else if (electrical_angle < 0.0F) {
01038       /* Outport: '<Root>/pwm_compare' incorporates:
01039        *  DataTypeConversion: '<S6>/Quantize'
01040        */
01041       pwm_compare[2] = 0U;
01042     } else {
01043       /* Outport: '<Root>/pwm_compare' incorporates:
01044        *  DataTypeConversion: '<S6>/Quantize'
01045        */
01046       pwm_compare[2] = (uint16_T)electrical_angle;
01047     }
01048   }
01049 
01050   /* End of Switch: '<S6>/Switch' */
01051 
01052   /* DeadZone: '<S19>/DeadZone' */
01053   if (SignDeltaU > 12.0F) {
01054     SignDeltaU -= 12.0F;
01055   } else if (SignDeltaU >= -12.0F) {
01056     SignDeltaU = 0.0F;
01057   } else {
01058     SignDeltaU -= -12.0F;
01059   }
01060 
01061   /* End of DeadZone: '<S19>/DeadZone' */
01062 
01063   /* RelationalOperator: '<S19>/NotEqual' */
01064   NotEqual_b = (0.0F != SignDeltaU);
01065 
01066   /* Signum: '<S19>/SignDeltaU' */
01067   if (SignDeltaU < 0.0F) {
01068     SignDeltaU = -1.0F;
01069   } else {
01070     if (SignDeltaU > 0.0F) {
01071       SignDeltaU = 1.0F;
01072     }
01073   }
01074 
01075   /* End of Signum: '<S19>/SignDeltaU' */
01076 
01077   /* Gain: '<S17>/Integral Gain' */
01078   IntegralGain_j = ctrlParams.Current_I * d_current_error;
01079 
01080   /* DataTypeConversion: '<S19>/DataTypeConv1' */
01081   if (SignDeltaU < 128.0F) {
01082     rtPrevAction = (int8_T)SignDeltaU;
01083   } else {
01084     rtPrevAction = MAX_int8_T;
01085   }
01086 
01087   /* End of DataTypeConversion: '<S19>/DataTypeConv1' */
01088 
01089   /* Signum: '<S19>/SignPreIntegrator' */
01090   if (IntegralGain_j < 0.0F) {
01091     electrical_angle = -1.0F;
01092   } else if (IntegralGain_j > 0.0F) {
01093     electrical_angle = 1.0F;
01094   } else {
01095     electrical_angle = IntegralGain_j;
01096   }
01097 
01098   /* Switch: '<S17>/Switch' incorporates:
01099    *  Constant: '<S17>/Constant'
01100    *  DataTypeConversion: '<S19>/DataTypeConv2'
01101    *  Logic: '<S19>/AND'
01102    *  RelationalOperator: '<S19>/Equal'
01103    *  Signum: '<S19>/SignPreIntegrator'
01104    */
01105   if (NotEqual_b && (rtPrevAction == ((int8_T)electrical_angle))) {
01106     electrical_angle = 0.0F;
01107   } else {
01108     electrical_angle = IntegralGain_j;
01109   }
01110 
01111   /* End of Switch: '<S17>/Switch' */
01112 
01113   /* DeadZone: '<S20>/DeadZone' */
01114   if (SignDeltaU_b > 12.0F) {
01115     SignDeltaU_b -= 12.0F;
01116   } else if (SignDeltaU_b >= -12.0F) {
01117     SignDeltaU_b = 0.0F;
01118   } else {
01119     SignDeltaU_b -= -12.0F;
01120   }
01121 
01122   /* End of DeadZone: '<S20>/DeadZone' */
01123 
01124   /* RelationalOperator: '<S20>/NotEqual' */
01125   NotEqual_b = (0.0F != SignDeltaU_b);
01126 
01127   /* Signum: '<S20>/SignDeltaU' */
01128   if (SignDeltaU_b < 0.0F) {
01129     SignDeltaU_b = -1.0F;
01130   } else {
01131     if (SignDeltaU_b > 0.0F) {
01132       SignDeltaU_b = 1.0F;
01133     }
01134   }
01135 
01136   /* End of Signum: '<S20>/SignDeltaU' */
01137 
01138   /* Gain: '<S18>/Integral Gain' */
01139   IntegralGain_j = ctrlParams.Current_I * q_current_error;
01140   if (M->Timing.TaskCounters.TID[1] == 0) {
01141     /* RelationalOperator: '<S6>/Relational Operator' incorporates:
01142      *  Constant: '<S47>/Constant'
01143      */
01144     RelationalOperator = (controller_mode == StandBy);
01145 
01146     /* Outputs for Enabled SubSystem: '<S58>/Generate_Error' incorporates:
01147      *  EnablePort: '<S60>/Enable'
01148      */
01149     /* Logic: '<S58>/AND' incorporates:
01150      *  Abs: '<S58>/Velocity_Abs'
01151      *  Constant: '<S58>/Max_Valid_Velocity_Change'
01152      *  Constant: '<S59>/Constant'
01153      *  Delay: '<S58>/Velocity_Delay'
01154      *  RelationalOperator: '<S58>/Excessive_Velocity_Change'
01155      *  RelationalOperator: '<S58>/Relational_Operator'
01156      *  Sum: '<S58>/Velocity_Difference'
01157      */
01158     if ((controller_mode == VelocityControl) && (((real32_T)fabs
01159           (velocity_measured - DWork.Velocity_Delay_DSTATE)) >= 628.318542F)) {
01160       /* DataStoreWrite: '<S60>/Data_Store_Write' incorporates:
01161        *  Constant: '<S61>/Constant'
01162        */
01163       DWork.error_l = MeasuredVelocityError;
01164     }
01165 
01166     /* End of Logic: '<S58>/AND' */
01167     /* End of Outputs for SubSystem: '<S58>/Generate_Error' */
01168   }
01169 
01170   /* Outport: '<Root>/error' incorporates:
01171    *  DataStoreRead: '<Root>/Data Store Read'
01172    */
01173   error = DWork.error_l;
01174 
01175   /* Update for RateTransition: '<S6>/Lo_to_Hi_Rate_Transition3' */
01176   if (M->Timing.TaskCounters.TID[1] == 0) {
01177     DWork.Lo_to_Hi_Rate_Transition3_Buffe = RelationalOperator;
01178 
01179     /* Update for Delay: '<S55>/Position_Delay' */
01180     DWork.Position_Delay_DSTATE = Wrap_To_Pi;
01181   }
01182 
01183   /* End of Update for RateTransition: '<S6>/Lo_to_Hi_Rate_Transition3' */
01184 
01185   /* Update for UnitDelay: '<S13>/Delay Input1' */
01186   DWork.DelayInput1_DSTATE = Enum_To_Int;
01187 
01188   /* Update for DiscreteIntegrator: '<S17>/Integrator' */
01189   if (FixPtRelationalOperator == 0) {
01190     DWork.Integrator_DSTATE += 4.0E-5F * electrical_angle;
01191   }
01192 
01193   if (FixPtRelationalOperator > 0) {
01194     DWork.Integrator_PrevResetState = 1;
01195   } else {
01196     DWork.Integrator_PrevResetState = 0;
01197   }
01198 
01199   /* End of Update for DiscreteIntegrator: '<S17>/Integrator' */
01200 
01201   /* Update for DiscreteIntegrator: '<S18>/Integrator' */
01202   if (FixPtRelationalOperator == 0) {
01203     /* DataTypeConversion: '<S20>/DataTypeConv1' */
01204     if (SignDeltaU_b < 128.0F) {
01205       rtPrevAction = (int8_T)SignDeltaU_b;
01206     } else {
01207       rtPrevAction = MAX_int8_T;
01208     }
01209 
01210     /* End of DataTypeConversion: '<S20>/DataTypeConv1' */
01211 
01212     /* Signum: '<S20>/SignPreIntegrator' */
01213     if (IntegralGain_j < 0.0F) {
01214       electrical_angle = -1.0F;
01215     } else if (IntegralGain_j > 0.0F) {
01216       electrical_angle = 1.0F;
01217     } else {
01218       electrical_angle = IntegralGain_j;
01219     }
01220 
01221     /* Switch: '<S18>/Switch' incorporates:
01222      *  Constant: '<S18>/Constant'
01223      *  DataTypeConversion: '<S20>/DataTypeConv2'
01224      *  Logic: '<S20>/AND'
01225      *  RelationalOperator: '<S20>/Equal'
01226      *  Signum: '<S20>/SignPreIntegrator'
01227      */
01228     if (NotEqual_b && (rtPrevAction == ((int8_T)electrical_angle))) {
01229       IntegralGain_j = 0.0F;
01230     }
01231 
01232     /* End of Switch: '<S18>/Switch' */
01233     DWork.Integrator_DSTATE_l += 4.0E-5F * IntegralGain_j;
01234   }
01235 
01236   if (FixPtRelationalOperator > 0) {
01237     DWork.Integrator_PrevResetState_c = 1;
01238   } else {
01239     DWork.Integrator_PrevResetState_c = 0;
01240   }
01241 
01242   /* End of Update for DiscreteIntegrator: '<S18>/Integrator' */
01243   if (M->Timing.TaskCounters.TID[1] == 0) {
01244     /* Update for Delay: '<S58>/Velocity_Delay' */
01245     DWork.Velocity_Delay_DSTATE = velocity_measured;
01246   }
01247 
01248   rate_scheduler();
01249   return error;
01250 }
01251 
01252 /* Model initialize function */
01253 extern "C" void Controller_Init(void)
01254 {
01255   /* Registration code */
01256 
01257   /* initialize real-time model */
01258   (void) memset((void *)M, 0,
01259                 sizeof(RT_MODEL));
01260 
01261   /* block I/O */
01262 
01263   /* exported global signals */
01264   phase_currents[0] = 0.0F;
01265   phase_currents[1] = 0.0F;
01266   rotor_position = 0.0F;
01267   velocity_measured = 0.0F;
01268   d_current_error = 0.0F;
01269   q_current_command = 0.0F;
01270   q_current_measured = 0.0F;
01271   q_current_error = 0.0F;
01272   phase_voltages[0] = 0.0F;
01273   phase_voltages[1] = 0.0F;
01274   phase_voltages[2] = 0.0F;
01275   velocity_error = 0.0F;
01276   controller_mode = StandBy;
01277 
01278   /* states (dwork) */
01279   (void) memset((void *)&DWork, 0,
01280                 sizeof(D_Work));
01281 
01282   /* InitializeConditions for Enabled SubSystem: '<S51>/Open Loop Position' */
01283   /* InitializeConditions for DiscreteIntegrator: '<S63>/Integrate_To_Position' */
01284   DWork.Integrate_To_Position_DSTATE = 0.0F;
01285 
01286   /* InitializeConditions for DiscreteIntegrator: '<S63>/Integrate_To_Velocity' */
01287   DWork.Integrate_To_Velocity_DSTATE = 0.0F;
01288 
01289   /* End of InitializeConditions for SubSystem: '<S51>/Open Loop Position' */
01290 
01291   /* Start for SwitchCase: '<S5>/Switch Case' */
01292   DWork.SwitchCase_ActiveSubsystem = -1;
01293 
01294   /* InitializeConditions for IfAction SubSystem: '<S5>/Velocity_Control' */
01295   /* InitializeConditions for DiscreteIntegrator: '<S45>/Integrator' */
01296   DWork.Integrator_DSTATE_f = 0.0F;
01297 
01298   /* End of InitializeConditions for SubSystem: '<S5>/Velocity_Control' */
01299 
01300   /* InitializeConditions for IfAction SubSystem: '<S5>/Position_Control' */
01301   /* InitializeConditions for DiscreteIntegrator: '<S42>/Integrator' */
01302   DWork.Integrator_DSTATE_lc = 0.0F;
01303 
01304   /* End of InitializeConditions for SubSystem: '<S5>/Position_Control' */
01305 
01306   /* InitializeConditions for Chart: '<S49>/Wait_For_Valid_Position' */
01307   DWork.temporalCounter_i1 = 0U;
01308   DWork.is_active_c2_rtwdemo_pmsmfoc = 0U;
01309   DWork.is_c2_rtwdemo_pmsmfoc = IN_NO_ACTIVE_CHILD;
01310   DWork.Position_Valid = 0U;
01311 
01312   /* InitializeConditions for Chart: '<S1>/Controller_Mode_Scheduler' */
01313   DWork.is_Motor_On = IN_NO_ACTIVE_CHILD;
01314   DWork.is_Motor_Control = IN_NO_ACTIVE_CHILD;
01315   DWork.velocity_command = 0.0F;
01316   DWork.position_command = 0.0F;
01317   DWork.torque_command = 0.0F;
01318 
01319   /* Entry: Mode_Scheduler/Controller_Mode_Scheduler */
01320   /* Entry Internal: Mode_Scheduler/Controller_Mode_Scheduler */
01321   /* Transition: '<S4>:9' */
01322   DWork.is_c1_rtwdemo_pmsmfoc = IN_Stand_By;
01323 
01324   /* Entry 'Stand_By': '<S4>:154' */
01325   controller_mode = StandBy;
01326 
01327   /* InitializeConditions for DiscreteIntegrator: '<S17>/Integrator' */
01328   DWork.Integrator_PrevResetState = 0;
01329 
01330   /* InitializeConditions for DiscreteIntegrator: '<S18>/Integrator' */
01331   DWork.Integrator_PrevResetState_c = 0;
01332 }
01333 
01334 /*
01335  * File trailer for generated code.
01336  *
01337  * [EOF]
01338  */