paul cox / Mbed 2 deprecated Nucleo_pmsmfoc

An example of importing Embedded Coder code into the mbed IDE. Currently doesn't connect IO to PWM, ADC, and Encoder, instead provides random inputs and measures execution time.

Dependencies:   mbed-dsp mbed Nucleo_pmsmfoc

Dependents:   Nucleo_pmsmfoc

rtwdemo_pmsmfoc.cpp@2:bbc155b0b886, 2014-11-25 (annotated)

Committer:
paulcox
Date:
Tue Nov 25 07:52:02 2014 +0000
Revision:
2:bbc155b0b886
Parent:
0:70d27fec6d71 
adding doc?

Who changed what in which revision?

UserRevisionLine numberNew contents of line
paulcox 0:70d27fec6d71 1 /*
paulcox 0:70d27fec6d71 2 * File: rtwdemo_pmsmfoc.c
paulcox 0:70d27fec6d71 3 *
paulcox 0:70d27fec6d71 4 * Code generated for Simulink model 'rtwdemo_pmsmfoc'.
paulcox 0:70d27fec6d71 5 *
paulcox 0:70d27fec6d71 6 * Model version : 1.2949
paulcox 0:70d27fec6d71 7 * Simulink Coder version : 8.7 (R2014b) 11-Aug-2014
paulcox 0:70d27fec6d71 8 * C/C++ source code generated on : Sat Oct 11 00:06:05 2014
paulcox 0:70d27fec6d71 9 *
paulcox 0:70d27fec6d71 10 * Target selection: ert.tlc
paulcox 0:70d27fec6d71 11 * Embedded hardware selection: Generic->32-bit Embedded Processor
paulcox 0:70d27fec6d71 12 * Code generation objective: Execution efficiency
paulcox 0:70d27fec6d71 13 * Validation result: Not run
paulcox 0:70d27fec6d71 14 */
paulcox 0:70d27fec6d71 15
paulcox 0:70d27fec6d71 16 #include "rtwdemo_pmsmfoc.h"
paulcox 2:bbc155b0b886 17 #include "arm_math.h"
paulcox 0:70d27fec6d71 18
paulcox 0:70d27fec6d71 19 /* Named constants for Chart: '<S1>/Controller_Mode_Scheduler' */
paulcox 0:70d27fec6d71 20 #define IN_Motor_Control ((uint8_T)1U)
paulcox 0:70d27fec6d71 21 #define IN_Motor_On ((uint8_T)1U)
paulcox 0:70d27fec6d71 22 #define IN_NO_ACTIVE_CHILD ((uint8_T)0U)
paulcox 0:70d27fec6d71 23 #define IN_Position_Control ((uint8_T)1U)
paulcox 0:70d27fec6d71 24 #define IN_Ramp_To_Stop ((uint8_T)2U)
paulcox 0:70d27fec6d71 25 #define IN_Stand_By ((uint8_T)2U)
paulcox 0:70d27fec6d71 26 #define IN_Startup_Open_Loop_Control ((uint8_T)2U)
paulcox 0:70d27fec6d71 27 #define IN_Torque_Control ((uint8_T)3U)
paulcox 0:70d27fec6d71 28 #define IN_Velocity_Control ((uint8_T)4U)
paulcox 0:70d27fec6d71 29
paulcox 0:70d27fec6d71 30 /* Named constants for Chart: '<S49>/Wait_For_Valid_Position' */
paulcox 0:70d27fec6d71 31 #define IN_Position_Valid ((uint8_T)1U)
paulcox 0:70d27fec6d71 32 #define IN_Wait_For_Valid_Position ((uint8_T)2U)
paulcox 0:70d27fec6d71 33 #define IN_Wait_For_Valid_Velocity ((uint8_T)3U)
paulcox 0:70d27fec6d71 34 #define WaitForValidVelocityTicks (124)
paulcox 0:70d27fec6d71 35
paulcox 0:70d27fec6d71 36 /* Exported block signals */
paulcox 0:70d27fec6d71 37 real32_T phase_currents[2]; /* '<S48>/Product' */
paulcox 0:70d27fec6d71 38 real32_T rotor_position; /* '<S49>/Add' */
paulcox 0:70d27fec6d71 39 real32_T velocity_measured; /* '<S55>/Scale_Output' */
paulcox 0:70d27fec6d71 40 real32_T d_current_error; /* '<S12>/Sum2' */
paulcox 0:70d27fec6d71 41 real32_T q_current_command; /* '<S5>/Lo_to_Hi_Rate_Transition1' */
paulcox 0:70d27fec6d71 42 real32_T q_current_measured; /* '<S22>/Add' */
paulcox 0:70d27fec6d71 43 real32_T q_current_error; /* '<S12>/Sum' */
paulcox 0:70d27fec6d71 44 real32_T phase_voltages[3]; /* '<S27>/Select_Sector' */
paulcox 0:70d27fec6d71 45 real32_T velocity_error; /* '<S11>/Sum2' */
paulcox 0:70d27fec6d71 46 EnumControllerMode controller_mode; /* '<S1>/Controller_Mode_Scheduler' */
paulcox 0:70d27fec6d71 47
paulcox 0:70d27fec6d71 48 /* Exported block parameters */
paulcox 0:70d27fec6d71 49 CTRLPARAMS_STRUCT ctrlParams = {
paulcox 0:70d27fec6d71 50 10.0F,
paulcox 0:70d27fec6d71 51 10000.0F,
paulcox 0:70d27fec6d71 52 0.005F,
paulcox 0:70d27fec6d71 53 0.015F,
paulcox 0:70d27fec6d71 54 0.1F,
paulcox 0:70d27fec6d71 55 0.6F,
paulcox 0:70d27fec6d71 56 1.0F,
paulcox 0:70d27fec6d71 57 0.2F,
paulcox 0:70d27fec6d71 58 20.0F,
paulcox 0:70d27fec6d71 59 2252.25F,
paulcox 0:70d27fec6d71 60 0.00488400506F,
paulcox 0:70d27fec6d71 61 -0.0F,
paulcox 0:70d27fec6d71 62 4.0F
paulcox 0:70d27fec6d71 63 } ; /* Variable: ctrlParams
paulcox 0:70d27fec6d71 64 * Referenced by:
paulcox 0:70d27fec6d71 65 * '<S1>/Controller_Mode_Scheduler'
paulcox 0:70d27fec6d71 66 * '<S48>/ADC_Driver_Units_To_Amps'
paulcox 0:70d27fec6d71 67 * '<S48>/ADC_Zero_Offset'
paulcox 0:70d27fec6d71 68 * '<S49>/Offset_Between_Encoder_Zero_And_Mechanical_Zero'
paulcox 0:70d27fec6d71 69 * '<S14>/number_of_pole_pairs'
paulcox 0:70d27fec6d71 70 * '<S42>/Integral Gain'
paulcox 0:70d27fec6d71 71 * '<S42>/Proportional Gain'
paulcox 0:70d27fec6d71 72 * '<S45>/Integral Gain'
paulcox 0:70d27fec6d71 73 * '<S45>/Proportional Gain'
paulcox 0:70d27fec6d71 74 * '<S63>/Startup_Acceleration_Constant'
paulcox 0:70d27fec6d71 75 * '<S17>/Integral Gain'
paulcox 0:70d27fec6d71 76 * '<S17>/Proportional Gain'
paulcox 0:70d27fec6d71 77 * '<S18>/Integral Gain'
paulcox 0:70d27fec6d71 78 * '<S18>/Proportional Gain'
paulcox 0:70d27fec6d71 79 */
paulcox 0:70d27fec6d71 80
paulcox 0:70d27fec6d71 81 /* Constant parameters (auto storage) */
paulcox 0:70d27fec6d71 82 const ConstParam ConstP = {
paulcox 0:70d27fec6d71 83 /* Computed Parameter: Lookup_Table_table
paulcox 0:70d27fec6d71 84 * Referenced by: '<S28>/Lookup_Table'
paulcox 0:70d27fec6d71 85 */
paulcox 0:70d27fec6d71 86 { 2U, 2U, 6U, 1U, 4U, 3U, 5U }
paulcox 0:70d27fec6d71 87 };
paulcox 0:70d27fec6d71 88
paulcox 0:70d27fec6d71 89 /* Block signals and states (auto storage) */
paulcox 0:70d27fec6d71 90 D_Work DWork;
paulcox 0:70d27fec6d71 91
paulcox 0:70d27fec6d71 92 /* Real-time model */
paulcox 0:70d27fec6d71 93 RT_MODEL M_;
paulcox 0:70d27fec6d71 94 RT_MODEL *const M = &M_;
paulcox 0:70d27fec6d71 95 extern real32_T rt_roundf(real32_T u);
paulcox 0:70d27fec6d71 96 extern real32_T rt_modf(real32_T u0, real32_T u1);
paulcox 0:70d27fec6d71 97 static void rate_scheduler(void);
paulcox 0:70d27fec6d71 98
paulcox 0:70d27fec6d71 99 /*
paulcox 0:70d27fec6d71 100 * This function updates active task flag for each subrate.
paulcox 0:70d27fec6d71 101 * The function is called at model base rate, hence the
paulcox 0:70d27fec6d71 102 * generated code self-manages all its subrates.
paulcox 0:70d27fec6d71 103 */
paulcox 0:70d27fec6d71 104 static void rate_scheduler(void)
paulcox 0:70d27fec6d71 105 {
paulcox 0:70d27fec6d71 106 /* Compute which subrates run during the next base time step. Subrates
paulcox 0:70d27fec6d71 107 * are an integer multiple of the base rate counter. Therefore, the subtask
paulcox 0:70d27fec6d71 108 * counter is reset when it reaches its limit (zero means run).
paulcox 0:70d27fec6d71 109 */
paulcox 0:70d27fec6d71 110 (M->Timing.TaskCounters.TID[1])++;
paulcox 0:70d27fec6d71 111 if ((M->Timing.TaskCounters.TID[1]) > 124) {/* Sample time: [0.005s, 0.0s] */
paulcox 0:70d27fec6d71 112 M->Timing.TaskCounters.TID[1] = 0;
paulcox 0:70d27fec6d71 113 }
paulcox 0:70d27fec6d71 114 }
paulcox 0:70d27fec6d71 115
paulcox 0:70d27fec6d71 116 real32_T rt_roundf(real32_T u)
paulcox 0:70d27fec6d71 117 {
paulcox 0:70d27fec6d71 118 real32_T y;
paulcox 0:70d27fec6d71 119 if (((real32_T)fabs(u)) < 8.388608E+6F) {
paulcox 0:70d27fec6d71 120 if (u >= 0.5F) {
paulcox 0:70d27fec6d71 121 y = (real32_T)floor(u + 0.5F);
paulcox 0:70d27fec6d71 122 } else if (u > -0.5F) {
paulcox 0:70d27fec6d71 123 y = 0.0F;
paulcox 0:70d27fec6d71 124 } else {
paulcox 0:70d27fec6d71 125 y = (real32_T)ceil(u - 0.5F);
paulcox 0:70d27fec6d71 126 }
paulcox 0:70d27fec6d71 127 } else {
paulcox 0:70d27fec6d71 128 y = u;
paulcox 0:70d27fec6d71 129 }
paulcox 0:70d27fec6d71 130
paulcox 0:70d27fec6d71 131 return y;
paulcox 0:70d27fec6d71 132 }
paulcox 0:70d27fec6d71 133
paulcox 0:70d27fec6d71 134 real32_T rt_modf(real32_T u0, real32_T u1)
paulcox 0:70d27fec6d71 135 {
paulcox 0:70d27fec6d71 136 real32_T y;
paulcox 0:70d27fec6d71 137 real32_T tmp;
paulcox 0:70d27fec6d71 138 if (u1 == 0.0F) {
paulcox 0:70d27fec6d71 139 y = u0;
paulcox 0:70d27fec6d71 140 } else {
paulcox 0:70d27fec6d71 141 tmp = u0 / u1;
paulcox 0:70d27fec6d71 142 if (u1 <= ((real32_T)floor(u1))) {
paulcox 0:70d27fec6d71 143 y = u0 - (((real32_T)floor(tmp)) * u1);
paulcox 0:70d27fec6d71 144 } else if (((real32_T)fabs(tmp - rt_roundf(tmp))) <= (FLT_EPSILON *
paulcox 0:70d27fec6d71 145 ((real32_T)fabs(tmp)))) {
paulcox 0:70d27fec6d71 146 y = 0.0F;
paulcox 0:70d27fec6d71 147 } else {
paulcox 0:70d27fec6d71 148 y = (tmp - ((real32_T)floor(tmp))) * u1;
paulcox 0:70d27fec6d71 149 }
paulcox 0:70d27fec6d71 150 }
paulcox 0:70d27fec6d71 151
paulcox 0:70d27fec6d71 152 return y;
paulcox 0:70d27fec6d71 153 }
paulcox 0:70d27fec6d71 154
paulcox 0:70d27fec6d71 155 /* Model step function */
paulcox 0:70d27fec6d71 156 extern "C" EnumErrorType Controller(uint16_T motor_on, EnumCommandType command_type,
paulcox 0:70d27fec6d71 157 real32_T current_request, SENSORS_STRUCT *sensors, uint16_T pwm_compare[3])
paulcox 0:70d27fec6d71 158 {
paulcox 0:70d27fec6d71 159 real32_T sin_coefficient;
paulcox 0:70d27fec6d71 160 int32_T Wrap_To_Pi;
paulcox 0:70d27fec6d71 161 real32_T electrical_angle;
paulcox 0:70d27fec6d71 162 real32_T cos_coefficient;
paulcox 0:70d27fec6d71 163 real32_T SignDeltaU_b;
paulcox 0:70d27fec6d71 164 int16_T Enum_To_Int;
paulcox 0:70d27fec6d71 165 uint8_T FixPtRelationalOperator;
paulcox 0:70d27fec6d71 166 real32_T SignDeltaU;
paulcox 0:70d27fec6d71 167 real32_T Gain1;
paulcox 0:70d27fec6d71 168 int8_T rtPrevAction;
paulcox 0:70d27fec6d71 169 real32_T alpha_voltage;
paulcox 0:70d27fec6d71 170 real32_T SignPreIntegrator_f;
paulcox 0:70d27fec6d71 171 real32_T IntegralGain_j;
paulcox 0:70d27fec6d71 172 boolean_T RelationalOperator;
paulcox 0:70d27fec6d71 173 boolean_T NotEqual_b;
paulcox 0:70d27fec6d71 174 real32_T Switch_fr;
paulcox 0:70d27fec6d71 175 real32_T Sectors_2_and_5_idx_1;
paulcox 0:70d27fec6d71 176 real32_T Sectors_2_and_5_idx_2;
paulcox 0:70d27fec6d71 177 int16_T u0;
paulcox 0:70d27fec6d71 178
paulcox 0:70d27fec6d71 179 /* specified return value */
paulcox 0:70d27fec6d71 180 EnumErrorType error;
paulcox 0:70d27fec6d71 181
paulcox 0:70d27fec6d71 182 /* Product: '<S48>/Product' incorporates:
paulcox 0:70d27fec6d71 183 * Constant: '<S48>/ADC_Driver_Units_To_Amps'
paulcox 0:70d27fec6d71 184 * Constant: '<S48>/ADC_Zero_Offset'
paulcox 0:70d27fec6d71 185 * Inport: '<Root>/sensors'
paulcox 0:70d27fec6d71 186 * Sum: '<S48>/Add'
paulcox 0:70d27fec6d71 187 */
paulcox 0:70d27fec6d71 188 phase_currents[0] = (((real32_T)sensors->adc_phase_currents[0]) -
paulcox 0:70d27fec6d71 189 ctrlParams.AdcZeroOffsetDriverUnits) *
paulcox 0:70d27fec6d71 190 ctrlParams.AdcDriverUnitsToAmps;
paulcox 0:70d27fec6d71 191 phase_currents[1] = (((real32_T)sensors->adc_phase_currents[1]) -
paulcox 0:70d27fec6d71 192 ctrlParams.AdcZeroOffsetDriverUnits) *
paulcox 0:70d27fec6d71 193 ctrlParams.AdcDriverUnitsToAmps;
paulcox 0:70d27fec6d71 194
paulcox 0:70d27fec6d71 195 /* Chart: '<S49>/Wait_For_Valid_Position' incorporates:
paulcox 0:70d27fec6d71 196 * Inport: '<Root>/sensors'
paulcox 0:70d27fec6d71 197 */
paulcox 0:70d27fec6d71 198 /* Gateway: Motor_Control/Sensors_To_Engineering_Units/Encoder_To_Position/Wait_For_Valid_Position */
paulcox 0:70d27fec6d71 199 if (DWork.temporalCounter_i1 < 127U) {
paulcox 0:70d27fec6d71 200 DWork.temporalCounter_i1++;
paulcox 0:70d27fec6d71 201 }
paulcox 0:70d27fec6d71 202
paulcox 0:70d27fec6d71 203 /* During: Motor_Control/Sensors_To_Engineering_Units/Encoder_To_Position/Wait_For_Valid_Position */
paulcox 0:70d27fec6d71 204 if (DWork.is_active_c2_rtwdemo_pmsmfoc == 0U) {
paulcox 0:70d27fec6d71 205 /* Entry: Motor_Control/Sensors_To_Engineering_Units/Encoder_To_Position/Wait_For_Valid_Position */
paulcox 0:70d27fec6d71 206 DWork.is_active_c2_rtwdemo_pmsmfoc = 1U;
paulcox 0:70d27fec6d71 207
paulcox 0:70d27fec6d71 208 /* Entry Internal: Motor_Control/Sensors_To_Engineering_Units/Encoder_To_Position/Wait_For_Valid_Position */
paulcox 0:70d27fec6d71 209 /* Transition: '<S52>:113' */
paulcox 0:70d27fec6d71 210 if (sensors->encoder_valid == 0) {
paulcox 0:70d27fec6d71 211 /* Transition: '<S52>:114' */
paulcox 0:70d27fec6d71 212 /* Transition: '<S52>:115' */
paulcox 0:70d27fec6d71 213 DWork.is_c2_rtwdemo_pmsmfoc = IN_Wait_For_Valid_Position;
paulcox 0:70d27fec6d71 214 } else {
paulcox 0:70d27fec6d71 215 /* Transition: '<S52>:120' */
paulcox 0:70d27fec6d71 216 /* Transition: '<S52>:119' */
paulcox 0:70d27fec6d71 217 DWork.is_c2_rtwdemo_pmsmfoc = IN_Position_Valid;
paulcox 0:70d27fec6d71 218 DWork.Position_Valid = 1U;
paulcox 0:70d27fec6d71 219 }
paulcox 0:70d27fec6d71 220 } else {
paulcox 0:70d27fec6d71 221 switch (DWork.is_c2_rtwdemo_pmsmfoc) {
paulcox 0:70d27fec6d71 222 case IN_Position_Valid:
paulcox 0:70d27fec6d71 223 /* During 'Position_Valid': '<S52>:25' */
paulcox 0:70d27fec6d71 224 break;
paulcox 0:70d27fec6d71 225
paulcox 0:70d27fec6d71 226 case IN_Wait_For_Valid_Position:
paulcox 0:70d27fec6d71 227 /* During 'Wait_For_Valid_Position': '<S52>:99' */
paulcox 0:70d27fec6d71 228 if (sensors->encoder_valid != 0) {
paulcox 0:70d27fec6d71 229 /* Transition: '<S52>:100' */
paulcox 0:70d27fec6d71 230 DWork.is_c2_rtwdemo_pmsmfoc = IN_Wait_For_Valid_Velocity;
paulcox 0:70d27fec6d71 231 DWork.temporalCounter_i1 = 0U;
paulcox 0:70d27fec6d71 232 }
paulcox 0:70d27fec6d71 233 break;
paulcox 0:70d27fec6d71 234
paulcox 0:70d27fec6d71 235 default:
paulcox 0:70d27fec6d71 236 /* During 'Wait_For_Valid_Velocity': '<S52>:101' */
paulcox 0:70d27fec6d71 237 if (DWork.temporalCounter_i1 >= WaitForValidVelocityTicks) {
paulcox 0:70d27fec6d71 238 /* Transition: '<S52>:102' */
paulcox 0:70d27fec6d71 239 DWork.is_c2_rtwdemo_pmsmfoc = IN_Position_Valid;
paulcox 0:70d27fec6d71 240 DWork.Position_Valid = 1U;
paulcox 0:70d27fec6d71 241 }
paulcox 0:70d27fec6d71 242 break;
paulcox 0:70d27fec6d71 243 }
paulcox 0:70d27fec6d71 244 }
paulcox 0:70d27fec6d71 245
paulcox 0:70d27fec6d71 246 /* End of Chart: '<S49>/Wait_For_Valid_Position' */
paulcox 0:70d27fec6d71 247
paulcox 0:70d27fec6d71 248 /* Sum: '<S49>/Add' incorporates:
paulcox 0:70d27fec6d71 249 * Constant: '<S49>/Offset_Between_Encoder_Zero_And_Mechanical_Zero'
paulcox 0:70d27fec6d71 250 * Gain: '<S49>/radians_per_counts'
paulcox 0:70d27fec6d71 251 * Inport: '<Root>/sensors'
paulcox 0:70d27fec6d71 252 */
paulcox 0:70d27fec6d71 253 rotor_position = (0.000785398181F * ((real32_T)sensors->encoder_counter)) +
paulcox 0:70d27fec6d71 254 ctrlParams.EncoderToMechanicalZeroOffsetRads;
paulcox 0:70d27fec6d71 255 if (M->Timing.TaskCounters.TID[1] == 0) {
paulcox 0:70d27fec6d71 256 /* Gain: '<S55>/Wrap_To_Pi' incorporates:
paulcox 0:70d27fec6d71 257 * DataTypeConversion: '<S55>/Convert_to_Uint32'
paulcox 0:70d27fec6d71 258 * Gain: '<S55>/Scale_Input'
paulcox 0:70d27fec6d71 259 */
paulcox 0:70d27fec6d71 260 Wrap_To_Pi = (((int32_T)(1.70891312E+8F * rotor_position)) << 2);
paulcox 0:70d27fec6d71 261
paulcox 0:70d27fec6d71 262 /* Gain: '<S55>/Scale_Output' incorporates:
paulcox 0:70d27fec6d71 263 * DataTypeConversion: '<S55>/Difference_to_Single'
paulcox 0:70d27fec6d71 264 * Delay: '<S55>/Position_Delay'
paulcox 0:70d27fec6d71 265 * Sum: '<S55>/Difference_Wrap'
paulcox 0:70d27fec6d71 266 */
paulcox 0:70d27fec6d71 267 velocity_measured = ((real32_T)(Wrap_To_Pi - DWork.Position_Delay_DSTATE)) *
paulcox 0:70d27fec6d71 268 2.92583621E-7F;
paulcox 0:70d27fec6d71 269
paulcox 0:70d27fec6d71 270 /* Chart: '<S1>/Controller_Mode_Scheduler' incorporates:
paulcox 0:70d27fec6d71 271 * Inport: '<Root>/command_type'
paulcox 0:70d27fec6d71 272 * Inport: '<Root>/command_value'
paulcox 0:70d27fec6d71 273 * Inport: '<Root>/motor_on'
paulcox 0:70d27fec6d71 274 */
paulcox 0:70d27fec6d71 275 /* Gateway: Mode_Scheduler/Controller_Mode_Scheduler */
paulcox 0:70d27fec6d71 276 /* During: Mode_Scheduler/Controller_Mode_Scheduler */
paulcox 0:70d27fec6d71 277 if (DWork.is_c1_rtwdemo_pmsmfoc == IN_Motor_On) {
paulcox 0:70d27fec6d71 278 /* During 'Motor_On': '<S4>:338' */
paulcox 0:70d27fec6d71 279 if (DWork.error_l != 0) {
paulcox 0:70d27fec6d71 280 /* Transition: '<S4>:339' */
paulcox 0:70d27fec6d71 281 /* Transition: '<S4>:353' */
paulcox 0:70d27fec6d71 282 /* Exit Internal 'Motor_On': '<S4>:338' */
paulcox 0:70d27fec6d71 283 /* Exit Internal 'Motor_Control': '<S4>:344' */
paulcox 0:70d27fec6d71 284 DWork.is_Motor_Control = IN_NO_ACTIVE_CHILD;
paulcox 0:70d27fec6d71 285 DWork.is_Motor_On = IN_NO_ACTIVE_CHILD;
paulcox 0:70d27fec6d71 286 DWork.is_c1_rtwdemo_pmsmfoc = IN_Stand_By;
paulcox 0:70d27fec6d71 287
paulcox 0:70d27fec6d71 288 /* Entry 'Stand_By': '<S4>:154' */
paulcox 0:70d27fec6d71 289 controller_mode = StandBy;
paulcox 0:70d27fec6d71 290 } else if (DWork.is_Motor_On == IN_Motor_Control) {
paulcox 0:70d27fec6d71 291 /* During 'Motor_Control': '<S4>:344' */
paulcox 0:70d27fec6d71 292 if (!(motor_on != 0)) {
paulcox 0:70d27fec6d71 293 /* Transition: '<S4>:282' */
paulcox 0:70d27fec6d71 294 /* Exit Internal 'Motor_Control': '<S4>:344' */
paulcox 0:70d27fec6d71 295 DWork.is_Motor_Control = IN_NO_ACTIVE_CHILD;
paulcox 0:70d27fec6d71 296 DWork.is_Motor_On = IN_Ramp_To_Stop;
paulcox 0:70d27fec6d71 297
paulcox 0:70d27fec6d71 298 /* Entry 'Ramp_To_Stop': '<S4>:270' */
paulcox 0:70d27fec6d71 299 controller_mode = VelocityControl;
paulcox 0:70d27fec6d71 300 DWork.velocity_command = 0.0F;
paulcox 0:70d27fec6d71 301 DWork.torque_command = 0.0F;
paulcox 0:70d27fec6d71 302 } else {
paulcox 0:70d27fec6d71 303 switch (DWork.is_Motor_Control) {
paulcox 0:70d27fec6d71 304 case IN_Position_Control:
paulcox 0:70d27fec6d71 305 /* During 'Position_Control': '<S4>:226' */
paulcox 0:70d27fec6d71 306 DWork.position_command = current_request;
paulcox 0:70d27fec6d71 307 break;
paulcox 0:70d27fec6d71 308
paulcox 0:70d27fec6d71 309 case IN_Startup_Open_Loop_Control:
paulcox 0:70d27fec6d71 310 /* During 'Startup_Open_Loop_Control': '<S4>:103' */
paulcox 0:70d27fec6d71 311 if (DWork.Position_Valid != 0) {
paulcox 0:70d27fec6d71 312 /* Transition: '<S4>:157' */
paulcox 0:70d27fec6d71 313 /* Transition: '<S4>:233' */
paulcox 0:70d27fec6d71 314 switch (command_type) {
paulcox 0:70d27fec6d71 315 case Velocity:
paulcox 0:70d27fec6d71 316 /* Transition: '<S4>:162' */
paulcox 0:70d27fec6d71 317 DWork.is_Motor_Control = IN_Velocity_Control;
paulcox 0:70d27fec6d71 318
paulcox 0:70d27fec6d71 319 /* Entry 'Velocity_Control': '<S4>:108' */
paulcox 0:70d27fec6d71 320 controller_mode = VelocityControl;
paulcox 0:70d27fec6d71 321 DWork.velocity_command = current_request;
paulcox 0:70d27fec6d71 322 break;
paulcox 0:70d27fec6d71 323
paulcox 0:70d27fec6d71 324 case Position:
paulcox 0:70d27fec6d71 325 /* Transition: '<S4>:235' */
paulcox 0:70d27fec6d71 326 /* Transition: '<S4>:234' */
paulcox 0:70d27fec6d71 327 DWork.is_Motor_Control = IN_Position_Control;
paulcox 0:70d27fec6d71 328
paulcox 0:70d27fec6d71 329 /* Entry 'Position_Control': '<S4>:226' */
paulcox 0:70d27fec6d71 330 controller_mode = PositionControl;
paulcox 0:70d27fec6d71 331 DWork.position_command = current_request;
paulcox 0:70d27fec6d71 332 break;
paulcox 0:70d27fec6d71 333
paulcox 0:70d27fec6d71 334 default:
paulcox 0:70d27fec6d71 335 /* Transition: '<S4>:237' */
paulcox 0:70d27fec6d71 336 /* Transition: '<S4>:158' */
paulcox 0:70d27fec6d71 337 /* [command_type==Torque] */
paulcox 0:70d27fec6d71 338 DWork.is_Motor_Control = IN_Torque_Control;
paulcox 0:70d27fec6d71 339
paulcox 0:70d27fec6d71 340 /* Entry 'Torque_Control': '<S4>:220' */
paulcox 0:70d27fec6d71 341 controller_mode = TorqueControl;
paulcox 0:70d27fec6d71 342 DWork.torque_command = current_request;
paulcox 0:70d27fec6d71 343 break;
paulcox 0:70d27fec6d71 344 }
paulcox 0:70d27fec6d71 345 }
paulcox 0:70d27fec6d71 346 break;
paulcox 0:70d27fec6d71 347
paulcox 0:70d27fec6d71 348 case IN_Torque_Control:
paulcox 0:70d27fec6d71 349 /* During 'Torque_Control': '<S4>:220' */
paulcox 0:70d27fec6d71 350 DWork.torque_command = current_request;
paulcox 0:70d27fec6d71 351 break;
paulcox 0:70d27fec6d71 352
paulcox 0:70d27fec6d71 353 default:
paulcox 0:70d27fec6d71 354 /* During 'Velocity_Control': '<S4>:108' */
paulcox 0:70d27fec6d71 355 DWork.velocity_command = current_request;
paulcox 0:70d27fec6d71 356 break;
paulcox 0:70d27fec6d71 357 }
paulcox 0:70d27fec6d71 358 }
paulcox 0:70d27fec6d71 359 } else {
paulcox 0:70d27fec6d71 360 /* During 'Ramp_To_Stop': '<S4>:270' */
paulcox 0:70d27fec6d71 361 if (((real32_T)fabs(velocity_measured)) < ctrlParams.RampToStopVelocity)
paulcox 0:70d27fec6d71 362 {
paulcox 0:70d27fec6d71 363 /* Transition: '<S4>:169' */
paulcox 0:70d27fec6d71 364 /* Transition: '<S4>:355' */
paulcox 0:70d27fec6d71 365 DWork.is_Motor_On = IN_NO_ACTIVE_CHILD;
paulcox 0:70d27fec6d71 366 DWork.is_c1_rtwdemo_pmsmfoc = IN_Stand_By;
paulcox 0:70d27fec6d71 367
paulcox 0:70d27fec6d71 368 /* Entry 'Stand_By': '<S4>:154' */
paulcox 0:70d27fec6d71 369 controller_mode = StandBy;
paulcox 0:70d27fec6d71 370 }
paulcox 0:70d27fec6d71 371 }
paulcox 0:70d27fec6d71 372 } else {
paulcox 0:70d27fec6d71 373 /* During 'Stand_By': '<S4>:154' */
paulcox 0:70d27fec6d71 374 if ((motor_on != 0) && (!(DWork.error_l != 0))) {
paulcox 0:70d27fec6d71 375 /* Transition: '<S4>:164' */
paulcox 0:70d27fec6d71 376 if (!(DWork.Position_Valid != 0)) {
paulcox 0:70d27fec6d71 377 /* Transition: '<S4>:133' */
paulcox 0:70d27fec6d71 378 DWork.is_c1_rtwdemo_pmsmfoc = IN_Motor_On;
paulcox 0:70d27fec6d71 379 DWork.is_Motor_On = IN_Motor_Control;
paulcox 0:70d27fec6d71 380 DWork.is_Motor_Control = IN_Startup_Open_Loop_Control;
paulcox 0:70d27fec6d71 381
paulcox 0:70d27fec6d71 382 /* Entry 'Startup_Open_Loop_Control': '<S4>:103' */
paulcox 0:70d27fec6d71 383 controller_mode = Startup;
paulcox 0:70d27fec6d71 384 DWork.torque_command = ctrlParams.StartupCurrent;
paulcox 0:70d27fec6d71 385 } else {
paulcox 0:70d27fec6d71 386 /* Transition: '<S4>:137' */
paulcox 0:70d27fec6d71 387 /* Transition: '<S4>:233' */
paulcox 0:70d27fec6d71 388 switch (command_type) {
paulcox 0:70d27fec6d71 389 case Velocity:
paulcox 0:70d27fec6d71 390 /* Transition: '<S4>:162' */
paulcox 0:70d27fec6d71 391 DWork.is_c1_rtwdemo_pmsmfoc = IN_Motor_On;
paulcox 0:70d27fec6d71 392 DWork.is_Motor_On = IN_Motor_Control;
paulcox 0:70d27fec6d71 393 DWork.is_Motor_Control = IN_Velocity_Control;
paulcox 0:70d27fec6d71 394
paulcox 0:70d27fec6d71 395 /* Entry 'Velocity_Control': '<S4>:108' */
paulcox 0:70d27fec6d71 396 controller_mode = VelocityControl;
paulcox 0:70d27fec6d71 397 DWork.velocity_command = current_request;
paulcox 0:70d27fec6d71 398 break;
paulcox 0:70d27fec6d71 399
paulcox 0:70d27fec6d71 400 case Position:
paulcox 0:70d27fec6d71 401 /* Transition: '<S4>:235' */
paulcox 0:70d27fec6d71 402 /* Transition: '<S4>:234' */
paulcox 0:70d27fec6d71 403 DWork.is_c1_rtwdemo_pmsmfoc = IN_Motor_On;
paulcox 0:70d27fec6d71 404 DWork.is_Motor_On = IN_Motor_Control;
paulcox 0:70d27fec6d71 405 DWork.is_Motor_Control = IN_Position_Control;
paulcox 0:70d27fec6d71 406
paulcox 0:70d27fec6d71 407 /* Entry 'Position_Control': '<S4>:226' */
paulcox 0:70d27fec6d71 408 controller_mode = PositionControl;
paulcox 0:70d27fec6d71 409 DWork.position_command = current_request;
paulcox 0:70d27fec6d71 410 break;
paulcox 0:70d27fec6d71 411
paulcox 0:70d27fec6d71 412 default:
paulcox 0:70d27fec6d71 413 /* Transition: '<S4>:237' */
paulcox 0:70d27fec6d71 414 /* Transition: '<S4>:158' */
paulcox 0:70d27fec6d71 415 /* [command_type==Torque] */
paulcox 0:70d27fec6d71 416 DWork.is_c1_rtwdemo_pmsmfoc = IN_Motor_On;
paulcox 0:70d27fec6d71 417 DWork.is_Motor_On = IN_Motor_Control;
paulcox 0:70d27fec6d71 418 DWork.is_Motor_Control = IN_Torque_Control;
paulcox 0:70d27fec6d71 419
paulcox 0:70d27fec6d71 420 /* Entry 'Torque_Control': '<S4>:220' */
paulcox 0:70d27fec6d71 421 controller_mode = TorqueControl;
paulcox 0:70d27fec6d71 422 DWork.torque_command = current_request;
paulcox 0:70d27fec6d71 423 break;
paulcox 0:70d27fec6d71 424 }
paulcox 0:70d27fec6d71 425 }
paulcox 0:70d27fec6d71 426 }
paulcox 0:70d27fec6d71 427 }
paulcox 0:70d27fec6d71 428
paulcox 0:70d27fec6d71 429 /* End of Chart: '<S1>/Controller_Mode_Scheduler' */
paulcox 0:70d27fec6d71 430
paulcox 0:70d27fec6d71 431 /* RelationalOperator: '<S51>/Relational Operator' incorporates:
paulcox 0:70d27fec6d71 432 * Constant: '<S62>/Constant'
paulcox 0:70d27fec6d71 433 */
paulcox 0:70d27fec6d71 434 DWork.RelationalOperator_a = (controller_mode == Startup);
paulcox 0:70d27fec6d71 435 }
paulcox 0:70d27fec6d71 436
paulcox 0:70d27fec6d71 437 /* Outputs for Enabled SubSystem: '<S51>/Open Loop Position' incorporates:
paulcox 0:70d27fec6d71 438 * EnablePort: '<S63>/Enable'
paulcox 0:70d27fec6d71 439 */
paulcox 0:70d27fec6d71 440 if (DWork.RelationalOperator_a) {
paulcox 0:70d27fec6d71 441 if (!DWork.OpenLoopPosition_MODE) {
paulcox 0:70d27fec6d71 442 /* InitializeConditions for DiscreteIntegrator: '<S63>/Integrate_To_Position' */
paulcox 0:70d27fec6d71 443 DWork.Integrate_To_Position_DSTATE = 0.0F;
paulcox 0:70d27fec6d71 444
paulcox 0:70d27fec6d71 445 /* InitializeConditions for DiscreteIntegrator: '<S63>/Integrate_To_Velocity' */
paulcox 0:70d27fec6d71 446 DWork.Integrate_To_Velocity_DSTATE = 0.0F;
paulcox 0:70d27fec6d71 447 DWork.OpenLoopPosition_MODE = true;
paulcox 0:70d27fec6d71 448 }
paulcox 0:70d27fec6d71 449
paulcox 0:70d27fec6d71 450 /* DiscreteIntegrator: '<S63>/Integrate_To_Position' */
paulcox 0:70d27fec6d71 451 DWork.position = DWork.Integrate_To_Position_DSTATE;
paulcox 0:70d27fec6d71 452
paulcox 0:70d27fec6d71 453 /* Update for DiscreteIntegrator: '<S63>/Integrate_To_Position' incorporates:
paulcox 0:70d27fec6d71 454 * DiscreteIntegrator: '<S63>/Integrate_To_Velocity'
paulcox 0:70d27fec6d71 455 */
paulcox 0:70d27fec6d71 456 DWork.Integrate_To_Position_DSTATE += 4.0E-5F *
paulcox 0:70d27fec6d71 457 DWork.Integrate_To_Velocity_DSTATE;
paulcox 0:70d27fec6d71 458
paulcox 0:70d27fec6d71 459 /* Update for DiscreteIntegrator: '<S63>/Integrate_To_Velocity' incorporates:
paulcox 0:70d27fec6d71 460 * Constant: '<S63>/Startup_Acceleration_Constant'
paulcox 0:70d27fec6d71 461 */
paulcox 0:70d27fec6d71 462 DWork.Integrate_To_Velocity_DSTATE += 4.0E-5F *
paulcox 0:70d27fec6d71 463 ctrlParams.StartupAcceleration;
paulcox 0:70d27fec6d71 464 } else {
paulcox 0:70d27fec6d71 465 if (DWork.OpenLoopPosition_MODE) {
paulcox 0:70d27fec6d71 466 DWork.OpenLoopPosition_MODE = false;
paulcox 0:70d27fec6d71 467 }
paulcox 0:70d27fec6d71 468 }
paulcox 0:70d27fec6d71 469
paulcox 0:70d27fec6d71 470 /* End of Outputs for SubSystem: '<S51>/Open Loop Position' */
paulcox 0:70d27fec6d71 471
paulcox 0:70d27fec6d71 472 /* Switch: '<S51>/Switch' */
paulcox 0:70d27fec6d71 473 if (DWork.RelationalOperator_a) {
paulcox 0:70d27fec6d71 474 Switch_fr = DWork.position;
paulcox 0:70d27fec6d71 475 } else {
paulcox 0:70d27fec6d71 476 Switch_fr = rotor_position;
paulcox 0:70d27fec6d71 477 }
paulcox 0:70d27fec6d71 478
paulcox 0:70d27fec6d71 479 /* End of Switch: '<S51>/Switch' */
paulcox 0:70d27fec6d71 480
paulcox 0:70d27fec6d71 481 /* Gain: '<S14>/number_of_pole_pairs' */
paulcox 0:70d27fec6d71 482 electrical_angle = ctrlParams.PmsmPolePairs * Switch_fr;
paulcox 0:70d27fec6d71 483
paulcox 0:70d27fec6d71 484 /* Trigonometry: '<S14>/sine_cosine' */
paulcox 2:bbc155b0b886 485 //sin_coefficient = (real32_T)sin(electrical_angle);
paulcox 2:bbc155b0b886 486 //cos_coefficient = (real32_T)cos(electrical_angle);
paulcox 2:bbc155b0b886 487 sin_coefficient = arm_sin_f32(electrical_angle);
paulcox 2:bbc155b0b886 488 cos_coefficient = arm_cos_f32(electrical_angle);
paulcox 2:bbc155b0b886 489
paulcox 0:70d27fec6d71 490 /* Gain: '<S21>/Beta_Gain' incorporates:
paulcox 0:70d27fec6d71 491 * Gain: '<S21>/B_Gain'
paulcox 0:70d27fec6d71 492 * Sum: '<S21>/Add'
paulcox 0:70d27fec6d71 493 */
paulcox 0:70d27fec6d71 494 SignDeltaU_b = ((2.0F * phase_currents[1]) + phase_currents[0]) * 0.577350259F;
paulcox 0:70d27fec6d71 495
paulcox 0:70d27fec6d71 496 /* Sum: '<S12>/Sum2' incorporates:
paulcox 0:70d27fec6d71 497 * Constant: '<S12>/d_current_command (A)'
paulcox 0:70d27fec6d71 498 * Product: '<S22>/Product2'
paulcox 0:70d27fec6d71 499 * Product: '<S22>/Product3'
paulcox 0:70d27fec6d71 500 * Sum: '<S22>/Add1'
paulcox 0:70d27fec6d71 501 */
paulcox 0:70d27fec6d71 502 d_current_error = 0.0F - ((phase_currents[0] * cos_coefficient) +
paulcox 0:70d27fec6d71 503 (SignDeltaU_b * sin_coefficient));
paulcox 0:70d27fec6d71 504
paulcox 0:70d27fec6d71 505 /* DataTypeConversion: '<S8>/Enum_To_Int' */
paulcox 0:70d27fec6d71 506 Enum_To_Int = (int16_T)controller_mode;
paulcox 0:70d27fec6d71 507
paulcox 0:70d27fec6d71 508 /* RelationalOperator: '<S13>/FixPt Relational Operator' incorporates:
paulcox 0:70d27fec6d71 509 * UnitDelay: '<S13>/Delay Input1'
paulcox 0:70d27fec6d71 510 */
paulcox 0:70d27fec6d71 511 FixPtRelationalOperator = (uint8_T)(Enum_To_Int != DWork.DelayInput1_DSTATE);
paulcox 0:70d27fec6d71 512
paulcox 0:70d27fec6d71 513 /* DiscreteIntegrator: '<S17>/Integrator' */
paulcox 0:70d27fec6d71 514 if ((FixPtRelationalOperator != 0) || (DWork.Integrator_PrevResetState != 0))
paulcox 0:70d27fec6d71 515 {
paulcox 0:70d27fec6d71 516 DWork.Integrator_DSTATE = 0.0F;
paulcox 0:70d27fec6d71 517 }
paulcox 0:70d27fec6d71 518
paulcox 0:70d27fec6d71 519 /* Sum: '<S17>/Sum' incorporates:
paulcox 0:70d27fec6d71 520 * DiscreteIntegrator: '<S17>/Integrator'
paulcox 0:70d27fec6d71 521 * Gain: '<S17>/Proportional Gain'
paulcox 0:70d27fec6d71 522 */
paulcox 0:70d27fec6d71 523 SignDeltaU = (ctrlParams.Current_P * d_current_error) +
paulcox 0:70d27fec6d71 524 DWork.Integrator_DSTATE;
paulcox 0:70d27fec6d71 525
paulcox 0:70d27fec6d71 526 /* Saturate: '<S17>/Saturate' */
paulcox 0:70d27fec6d71 527 if (SignDeltaU > 12.0F) {
paulcox 0:70d27fec6d71 528 Gain1 = 12.0F;
paulcox 0:70d27fec6d71 529 } else if (SignDeltaU < -12.0F) {
paulcox 0:70d27fec6d71 530 Gain1 = -12.0F;
paulcox 0:70d27fec6d71 531 } else {
paulcox 0:70d27fec6d71 532 Gain1 = SignDeltaU;
paulcox 0:70d27fec6d71 533 }
paulcox 0:70d27fec6d71 534
paulcox 0:70d27fec6d71 535 /* End of Saturate: '<S17>/Saturate' */
paulcox 0:70d27fec6d71 536 if (M->Timing.TaskCounters.TID[1] == 0) {
paulcox 0:70d27fec6d71 537 /* SwitchCase: '<S5>/Switch Case' incorporates:
paulcox 0:70d27fec6d71 538 * Inport: '<S10>/torque_command'
paulcox 0:70d27fec6d71 539 */
paulcox 0:70d27fec6d71 540 rtPrevAction = DWork.SwitchCase_ActiveSubsystem;
paulcox 0:70d27fec6d71 541 switch (controller_mode) {
paulcox 0:70d27fec6d71 542 case VelocityControl:
paulcox 0:70d27fec6d71 543 DWork.SwitchCase_ActiveSubsystem = 0;
paulcox 0:70d27fec6d71 544 break;
paulcox 0:70d27fec6d71 545
paulcox 0:70d27fec6d71 546 case PositionControl:
paulcox 0:70d27fec6d71 547 DWork.SwitchCase_ActiveSubsystem = 1;
paulcox 0:70d27fec6d71 548 break;
paulcox 0:70d27fec6d71 549
paulcox 0:70d27fec6d71 550 default:
paulcox 0:70d27fec6d71 551 DWork.SwitchCase_ActiveSubsystem = 2;
paulcox 0:70d27fec6d71 552 break;
paulcox 0:70d27fec6d71 553 }
paulcox 0:70d27fec6d71 554
paulcox 0:70d27fec6d71 555 switch (DWork.SwitchCase_ActiveSubsystem) {
paulcox 0:70d27fec6d71 556 case 0:
paulcox 0:70d27fec6d71 557 if (DWork.SwitchCase_ActiveSubsystem != rtPrevAction) {
paulcox 0:70d27fec6d71 558 /* InitializeConditions for IfAction SubSystem: '<S5>/Velocity_Control' incorporates:
paulcox 0:70d27fec6d71 559 * InitializeConditions for ActionPort: '<S11>/Action Port'
paulcox 0:70d27fec6d71 560 */
paulcox 0:70d27fec6d71 561 /* InitializeConditions for SwitchCase: '<S5>/Switch Case' incorporates:
paulcox 0:70d27fec6d71 562 * InitializeConditions for DiscreteIntegrator: '<S45>/Integrator'
paulcox 0:70d27fec6d71 563 */
paulcox 0:70d27fec6d71 564 DWork.Integrator_DSTATE_f = 0.0F;
paulcox 0:70d27fec6d71 565
paulcox 0:70d27fec6d71 566 /* End of InitializeConditions for SubSystem: '<S5>/Velocity_Control' */
paulcox 0:70d27fec6d71 567 }
paulcox 0:70d27fec6d71 568
paulcox 0:70d27fec6d71 569 /* Outputs for IfAction SubSystem: '<S5>/Velocity_Control' incorporates:
paulcox 0:70d27fec6d71 570 * ActionPort: '<S11>/Action Port'
paulcox 0:70d27fec6d71 571 */
paulcox 0:70d27fec6d71 572 /* Sum: '<S11>/Sum2' */
paulcox 0:70d27fec6d71 573 velocity_error = DWork.velocity_command - velocity_measured;
paulcox 0:70d27fec6d71 574
paulcox 0:70d27fec6d71 575 /* Sum: '<S45>/Sum' incorporates:
paulcox 0:70d27fec6d71 576 * DiscreteIntegrator: '<S45>/Integrator'
paulcox 0:70d27fec6d71 577 * Gain: '<S45>/Proportional Gain'
paulcox 0:70d27fec6d71 578 */
paulcox 0:70d27fec6d71 579 electrical_angle = (ctrlParams.Velocity_P * velocity_error) +
paulcox 0:70d27fec6d71 580 DWork.Integrator_DSTATE_f;
paulcox 0:70d27fec6d71 581
paulcox 0:70d27fec6d71 582 /* Saturate: '<S45>/Saturate' */
paulcox 0:70d27fec6d71 583 if (electrical_angle > 2.0F) {
paulcox 0:70d27fec6d71 584 /* SignalConversion: '<S11>/Isolate_For_Merge' */
paulcox 0:70d27fec6d71 585 DWork.Merge = 2.0F;
paulcox 0:70d27fec6d71 586 } else if (electrical_angle < -2.0F) {
paulcox 0:70d27fec6d71 587 /* SignalConversion: '<S11>/Isolate_For_Merge' */
paulcox 0:70d27fec6d71 588 DWork.Merge = -2.0F;
paulcox 0:70d27fec6d71 589 } else {
paulcox 0:70d27fec6d71 590 /* SignalConversion: '<S11>/Isolate_For_Merge' */
paulcox 0:70d27fec6d71 591 DWork.Merge = electrical_angle;
paulcox 0:70d27fec6d71 592 }
paulcox 0:70d27fec6d71 593
paulcox 0:70d27fec6d71 594 /* End of Saturate: '<S45>/Saturate' */
paulcox 0:70d27fec6d71 595
paulcox 0:70d27fec6d71 596 /* DeadZone: '<S46>/DeadZone' */
paulcox 0:70d27fec6d71 597 if (electrical_angle > 2.0F) {
paulcox 0:70d27fec6d71 598 electrical_angle -= 2.0F;
paulcox 0:70d27fec6d71 599 } else if (electrical_angle >= -2.0F) {
paulcox 0:70d27fec6d71 600 electrical_angle = 0.0F;
paulcox 0:70d27fec6d71 601 } else {
paulcox 0:70d27fec6d71 602 electrical_angle -= -2.0F;
paulcox 0:70d27fec6d71 603 }
paulcox 0:70d27fec6d71 604
paulcox 0:70d27fec6d71 605 /* End of DeadZone: '<S46>/DeadZone' */
paulcox 0:70d27fec6d71 606
paulcox 0:70d27fec6d71 607 /* RelationalOperator: '<S46>/NotEqual' */
paulcox 0:70d27fec6d71 608 NotEqual_b = (0.0F != electrical_angle);
paulcox 0:70d27fec6d71 609
paulcox 0:70d27fec6d71 610 /* Signum: '<S46>/SignDeltaU' */
paulcox 0:70d27fec6d71 611 if (electrical_angle < 0.0F) {
paulcox 0:70d27fec6d71 612 electrical_angle = -1.0F;
paulcox 0:70d27fec6d71 613 } else {
paulcox 0:70d27fec6d71 614 if (electrical_angle > 0.0F) {
paulcox 0:70d27fec6d71 615 electrical_angle = 1.0F;
paulcox 0:70d27fec6d71 616 }
paulcox 0:70d27fec6d71 617 }
paulcox 0:70d27fec6d71 618
paulcox 0:70d27fec6d71 619 /* End of Signum: '<S46>/SignDeltaU' */
paulcox 0:70d27fec6d71 620
paulcox 0:70d27fec6d71 621 /* Gain: '<S45>/Integral Gain' */
paulcox 0:70d27fec6d71 622 Switch_fr = ctrlParams.Velocity_I * velocity_error;
paulcox 0:70d27fec6d71 623
paulcox 0:70d27fec6d71 624 /* DataTypeConversion: '<S46>/DataTypeConv1' */
paulcox 0:70d27fec6d71 625 if (electrical_angle < 128.0F) {
paulcox 0:70d27fec6d71 626 rtPrevAction = (int8_T)electrical_angle;
paulcox 0:70d27fec6d71 627 } else {
paulcox 0:70d27fec6d71 628 rtPrevAction = MAX_int8_T;
paulcox 0:70d27fec6d71 629 }
paulcox 0:70d27fec6d71 630
paulcox 0:70d27fec6d71 631 /* End of DataTypeConversion: '<S46>/DataTypeConv1' */
paulcox 0:70d27fec6d71 632
paulcox 0:70d27fec6d71 633 /* Signum: '<S46>/SignPreIntegrator' */
paulcox 0:70d27fec6d71 634 if (Switch_fr < 0.0F) {
paulcox 0:70d27fec6d71 635 electrical_angle = -1.0F;
paulcox 0:70d27fec6d71 636 } else if (Switch_fr > 0.0F) {
paulcox 0:70d27fec6d71 637 electrical_angle = 1.0F;
paulcox 0:70d27fec6d71 638 } else {
paulcox 0:70d27fec6d71 639 electrical_angle = Switch_fr;
paulcox 0:70d27fec6d71 640 }
paulcox 0:70d27fec6d71 641
paulcox 0:70d27fec6d71 642 /* Switch: '<S45>/Switch' incorporates:
paulcox 0:70d27fec6d71 643 * Constant: '<S45>/Constant'
paulcox 0:70d27fec6d71 644 * DataTypeConversion: '<S46>/DataTypeConv2'
paulcox 0:70d27fec6d71 645 * Logic: '<S46>/AND'
paulcox 0:70d27fec6d71 646 * RelationalOperator: '<S46>/Equal'
paulcox 0:70d27fec6d71 647 * Signum: '<S46>/SignPreIntegrator'
paulcox 0:70d27fec6d71 648 */
paulcox 0:70d27fec6d71 649 if (NotEqual_b && (rtPrevAction == ((int8_T)electrical_angle))) {
paulcox 0:70d27fec6d71 650 Switch_fr = 0.0F;
paulcox 0:70d27fec6d71 651 }
paulcox 0:70d27fec6d71 652
paulcox 0:70d27fec6d71 653 /* End of Switch: '<S45>/Switch' */
paulcox 0:70d27fec6d71 654
paulcox 0:70d27fec6d71 655 /* Update for DiscreteIntegrator: '<S45>/Integrator' */
paulcox 0:70d27fec6d71 656 DWork.Integrator_DSTATE_f += 0.005F * Switch_fr;
paulcox 0:70d27fec6d71 657
paulcox 0:70d27fec6d71 658 /* End of Outputs for SubSystem: '<S5>/Velocity_Control' */
paulcox 0:70d27fec6d71 659 break;
paulcox 0:70d27fec6d71 660
paulcox 0:70d27fec6d71 661 case 1:
paulcox 0:70d27fec6d71 662 if (DWork.SwitchCase_ActiveSubsystem != rtPrevAction) {
paulcox 0:70d27fec6d71 663 /* InitializeConditions for IfAction SubSystem: '<S5>/Position_Control' incorporates:
paulcox 0:70d27fec6d71 664 * InitializeConditions for ActionPort: '<S9>/Action Port'
paulcox 0:70d27fec6d71 665 */
paulcox 0:70d27fec6d71 666 /* InitializeConditions for SwitchCase: '<S5>/Switch Case' incorporates:
paulcox 0:70d27fec6d71 667 * InitializeConditions for DiscreteIntegrator: '<S42>/Integrator'
paulcox 0:70d27fec6d71 668 */
paulcox 0:70d27fec6d71 669 DWork.Integrator_DSTATE_lc = 0.0F;
paulcox 0:70d27fec6d71 670
paulcox 0:70d27fec6d71 671 /* End of InitializeConditions for SubSystem: '<S5>/Position_Control' */
paulcox 0:70d27fec6d71 672 }
paulcox 0:70d27fec6d71 673
paulcox 0:70d27fec6d71 674 /* Outputs for IfAction SubSystem: '<S5>/Position_Control' incorporates:
paulcox 0:70d27fec6d71 675 * ActionPort: '<S9>/Action Port'
paulcox 0:70d27fec6d71 676 */
paulcox 0:70d27fec6d71 677 /* Sum: '<S9>/Sum2' */
paulcox 0:70d27fec6d71 678 Switch_fr = DWork.position_command - Switch_fr;
paulcox 0:70d27fec6d71 679
paulcox 0:70d27fec6d71 680 /* Switch: '<S43>/Select_Angle' incorporates:
paulcox 0:70d27fec6d71 681 * Constant: '<S43>/Neg_Pi_Constant'
paulcox 0:70d27fec6d71 682 * Constant: '<S43>/Pi_Constant_1'
paulcox 0:70d27fec6d71 683 * Constant: '<S43>/Pi_Constant_2'
paulcox 0:70d27fec6d71 684 * Constant: '<S43>/Pi_Constant_3'
paulcox 0:70d27fec6d71 685 * Constant: '<S43>/Two_Pi_Constant'
paulcox 0:70d27fec6d71 686 * Logic: '<S43>/OR'
paulcox 0:70d27fec6d71 687 * Math: '<S43>/Modulus'
paulcox 0:70d27fec6d71 688 * RelationalOperator: '<S43>/Greater_Than'
paulcox 0:70d27fec6d71 689 * RelationalOperator: '<S43>/Less_Than'
paulcox 0:70d27fec6d71 690 * Sum: '<S43>/Add'
paulcox 0:70d27fec6d71 691 * Sum: '<S43>/Subtract'
paulcox 0:70d27fec6d71 692 */
paulcox 0:70d27fec6d71 693 if ((Switch_fr < -1.57079637F) || (Switch_fr >= 1.57079637F)) {
paulcox 0:70d27fec6d71 694 Switch_fr = rt_modf(Switch_fr + 1.57079637F, 3.14159274F) - 1.57079637F;
paulcox 0:70d27fec6d71 695 }
paulcox 0:70d27fec6d71 696
paulcox 0:70d27fec6d71 697 /* End of Switch: '<S43>/Select_Angle' */
paulcox 0:70d27fec6d71 698
paulcox 0:70d27fec6d71 699 /* Sum: '<S42>/Sum' incorporates:
paulcox 0:70d27fec6d71 700 * DiscreteIntegrator: '<S42>/Integrator'
paulcox 0:70d27fec6d71 701 * Gain: '<S42>/Proportional Gain'
paulcox 0:70d27fec6d71 702 */
paulcox 0:70d27fec6d71 703 electrical_angle = (ctrlParams.Position_P * Switch_fr) +
paulcox 0:70d27fec6d71 704 DWork.Integrator_DSTATE_lc;
paulcox 0:70d27fec6d71 705
paulcox 0:70d27fec6d71 706 /* Saturate: '<S42>/Saturate' */
paulcox 0:70d27fec6d71 707 if (electrical_angle > 2.0F) {
paulcox 0:70d27fec6d71 708 /* SignalConversion: '<S9>/Isolate_For_Merge' */
paulcox 0:70d27fec6d71 709 DWork.Merge = 2.0F;
paulcox 0:70d27fec6d71 710 } else if (electrical_angle < -2.0F) {
paulcox 0:70d27fec6d71 711 /* SignalConversion: '<S9>/Isolate_For_Merge' */
paulcox 0:70d27fec6d71 712 DWork.Merge = -2.0F;
paulcox 0:70d27fec6d71 713 } else {
paulcox 0:70d27fec6d71 714 /* SignalConversion: '<S9>/Isolate_For_Merge' */
paulcox 0:70d27fec6d71 715 DWork.Merge = electrical_angle;
paulcox 0:70d27fec6d71 716 }
paulcox 0:70d27fec6d71 717
paulcox 0:70d27fec6d71 718 /* End of Saturate: '<S42>/Saturate' */
paulcox 0:70d27fec6d71 719
paulcox 0:70d27fec6d71 720 /* DeadZone: '<S44>/DeadZone' */
paulcox 0:70d27fec6d71 721 if (electrical_angle > 2.0F) {
paulcox 0:70d27fec6d71 722 electrical_angle -= 2.0F;
paulcox 0:70d27fec6d71 723 } else if (electrical_angle >= -2.0F) {
paulcox 0:70d27fec6d71 724 electrical_angle = 0.0F;
paulcox 0:70d27fec6d71 725 } else {
paulcox 0:70d27fec6d71 726 electrical_angle -= -2.0F;
paulcox 0:70d27fec6d71 727 }
paulcox 0:70d27fec6d71 728
paulcox 0:70d27fec6d71 729 /* End of DeadZone: '<S44>/DeadZone' */
paulcox 0:70d27fec6d71 730
paulcox 0:70d27fec6d71 731 /* RelationalOperator: '<S44>/NotEqual' */
paulcox 0:70d27fec6d71 732 NotEqual_b = (0.0F != electrical_angle);
paulcox 0:70d27fec6d71 733
paulcox 0:70d27fec6d71 734 /* Signum: '<S44>/SignDeltaU' */
paulcox 0:70d27fec6d71 735 if (electrical_angle < 0.0F) {
paulcox 0:70d27fec6d71 736 electrical_angle = -1.0F;
paulcox 0:70d27fec6d71 737 } else {
paulcox 0:70d27fec6d71 738 if (electrical_angle > 0.0F) {
paulcox 0:70d27fec6d71 739 electrical_angle = 1.0F;
paulcox 0:70d27fec6d71 740 }
paulcox 0:70d27fec6d71 741 }
paulcox 0:70d27fec6d71 742
paulcox 0:70d27fec6d71 743 /* End of Signum: '<S44>/SignDeltaU' */
paulcox 0:70d27fec6d71 744
paulcox 0:70d27fec6d71 745 /* Gain: '<S42>/Integral Gain' */
paulcox 0:70d27fec6d71 746 Switch_fr *= ctrlParams.Position_I;
paulcox 0:70d27fec6d71 747
paulcox 0:70d27fec6d71 748 /* DataTypeConversion: '<S44>/DataTypeConv1' */
paulcox 0:70d27fec6d71 749 if (electrical_angle < 128.0F) {
paulcox 0:70d27fec6d71 750 rtPrevAction = (int8_T)electrical_angle;
paulcox 0:70d27fec6d71 751 } else {
paulcox 0:70d27fec6d71 752 rtPrevAction = MAX_int8_T;
paulcox 0:70d27fec6d71 753 }
paulcox 0:70d27fec6d71 754
paulcox 0:70d27fec6d71 755 /* End of DataTypeConversion: '<S44>/DataTypeConv1' */
paulcox 0:70d27fec6d71 756
paulcox 0:70d27fec6d71 757 /* Signum: '<S44>/SignPreIntegrator' */
paulcox 0:70d27fec6d71 758 if (Switch_fr < 0.0F) {
paulcox 0:70d27fec6d71 759 electrical_angle = -1.0F;
paulcox 0:70d27fec6d71 760 } else if (Switch_fr > 0.0F) {
paulcox 0:70d27fec6d71 761 electrical_angle = 1.0F;
paulcox 0:70d27fec6d71 762 } else {
paulcox 0:70d27fec6d71 763 electrical_angle = Switch_fr;
paulcox 0:70d27fec6d71 764 }
paulcox 0:70d27fec6d71 765
paulcox 0:70d27fec6d71 766 /* Switch: '<S42>/Switch' incorporates:
paulcox 0:70d27fec6d71 767 * Constant: '<S42>/Constant'
paulcox 0:70d27fec6d71 768 * DataTypeConversion: '<S44>/DataTypeConv2'
paulcox 0:70d27fec6d71 769 * Logic: '<S44>/AND'
paulcox 0:70d27fec6d71 770 * RelationalOperator: '<S44>/Equal'
paulcox 0:70d27fec6d71 771 * Signum: '<S44>/SignPreIntegrator'
paulcox 0:70d27fec6d71 772 */
paulcox 0:70d27fec6d71 773 if (NotEqual_b && (rtPrevAction == ((int8_T)electrical_angle))) {
paulcox 0:70d27fec6d71 774 Switch_fr = 0.0F;
paulcox 0:70d27fec6d71 775 }
paulcox 0:70d27fec6d71 776
paulcox 0:70d27fec6d71 777 /* End of Switch: '<S42>/Switch' */
paulcox 0:70d27fec6d71 778
paulcox 0:70d27fec6d71 779 /* Update for DiscreteIntegrator: '<S42>/Integrator' */
paulcox 0:70d27fec6d71 780 DWork.Integrator_DSTATE_lc += 0.005F * Switch_fr;
paulcox 0:70d27fec6d71 781
paulcox 0:70d27fec6d71 782 /* End of Outputs for SubSystem: '<S5>/Position_Control' */
paulcox 0:70d27fec6d71 783 break;
paulcox 0:70d27fec6d71 784
paulcox 0:70d27fec6d71 785 case 2:
paulcox 0:70d27fec6d71 786 /* Outputs for IfAction SubSystem: '<S5>/Torque_Control' incorporates:
paulcox 0:70d27fec6d71 787 * ActionPort: '<S10>/Action Port'
paulcox 0:70d27fec6d71 788 */
paulcox 0:70d27fec6d71 789 DWork.Merge = DWork.torque_command;
paulcox 0:70d27fec6d71 790
paulcox 0:70d27fec6d71 791 /* End of Outputs for SubSystem: '<S5>/Torque_Control' */
paulcox 0:70d27fec6d71 792 break;
paulcox 0:70d27fec6d71 793 }
paulcox 0:70d27fec6d71 794
paulcox 0:70d27fec6d71 795 /* End of SwitchCase: '<S5>/Switch Case' */
paulcox 0:70d27fec6d71 796 }
paulcox 0:70d27fec6d71 797
paulcox 0:70d27fec6d71 798 /* RateTransition: '<S5>/Lo_to_Hi_Rate_Transition1' */
paulcox 0:70d27fec6d71 799 q_current_command = DWork.Merge;
paulcox 0:70d27fec6d71 800
paulcox 0:70d27fec6d71 801 /* Sum: '<S22>/Add' incorporates:
paulcox 0:70d27fec6d71 802 * Product: '<S22>/Product'
paulcox 0:70d27fec6d71 803 * Product: '<S22>/Product1'
paulcox 0:70d27fec6d71 804 */
paulcox 0:70d27fec6d71 805 q_current_measured = (SignDeltaU_b * cos_coefficient) - (phase_currents[0] *
paulcox 0:70d27fec6d71 806 sin_coefficient);
paulcox 0:70d27fec6d71 807
paulcox 0:70d27fec6d71 808 /* Sum: '<S12>/Sum' */
paulcox 0:70d27fec6d71 809 q_current_error = q_current_command - q_current_measured;
paulcox 0:70d27fec6d71 810
paulcox 0:70d27fec6d71 811 /* DiscreteIntegrator: '<S18>/Integrator' */
paulcox 0:70d27fec6d71 812 if ((FixPtRelationalOperator != 0) || (DWork.Integrator_PrevResetState_c != 0))
paulcox 0:70d27fec6d71 813 {
paulcox 0:70d27fec6d71 814 DWork.Integrator_DSTATE_l = 0.0F;
paulcox 0:70d27fec6d71 815 }
paulcox 0:70d27fec6d71 816
paulcox 0:70d27fec6d71 817 /* Sum: '<S18>/Sum' incorporates:
paulcox 0:70d27fec6d71 818 * DiscreteIntegrator: '<S18>/Integrator'
paulcox 0:70d27fec6d71 819 * Gain: '<S18>/Proportional Gain'
paulcox 0:70d27fec6d71 820 */
paulcox 0:70d27fec6d71 821 SignDeltaU_b = (ctrlParams.Current_P * q_current_error) +
paulcox 0:70d27fec6d71 822 DWork.Integrator_DSTATE_l;
paulcox 0:70d27fec6d71 823
paulcox 0:70d27fec6d71 824 /* Saturate: '<S18>/Saturate' */
paulcox 0:70d27fec6d71 825 if (SignDeltaU_b > 12.0F) {
paulcox 0:70d27fec6d71 826 alpha_voltage = 12.0F;
paulcox 0:70d27fec6d71 827 } else if (SignDeltaU_b < -12.0F) {
paulcox 0:70d27fec6d71 828 alpha_voltage = -12.0F;
paulcox 0:70d27fec6d71 829 } else {
paulcox 0:70d27fec6d71 830 alpha_voltage = SignDeltaU_b;
paulcox 0:70d27fec6d71 831 }
paulcox 0:70d27fec6d71 832
paulcox 0:70d27fec6d71 833 /* End of Saturate: '<S18>/Saturate' */
paulcox 0:70d27fec6d71 834
paulcox 0:70d27fec6d71 835 /* Sum: '<S24>/Add' incorporates:
paulcox 0:70d27fec6d71 836 * Product: '<S24>/Product'
paulcox 0:70d27fec6d71 837 * Product: '<S24>/Product1'
paulcox 0:70d27fec6d71 838 */
paulcox 0:70d27fec6d71 839 SignPreIntegrator_f = (Gain1 * sin_coefficient) + (alpha_voltage *
paulcox 0:70d27fec6d71 840 cos_coefficient);
paulcox 0:70d27fec6d71 841
paulcox 0:70d27fec6d71 842 /* Gain: '<S29>/Gain' */
paulcox 0:70d27fec6d71 843 IntegralGain_j = 0.5F * SignPreIntegrator_f;
paulcox 0:70d27fec6d71 844
paulcox 0:70d27fec6d71 845 /* Sum: '<S24>/Add1' incorporates:
paulcox 0:70d27fec6d71 846 * Product: '<S24>/Product2'
paulcox 0:70d27fec6d71 847 * Product: '<S24>/Product3'
paulcox 0:70d27fec6d71 848 */
paulcox 0:70d27fec6d71 849 alpha_voltage = (Gain1 * cos_coefficient) - (alpha_voltage * sin_coefficient);
paulcox 0:70d27fec6d71 850
paulcox 0:70d27fec6d71 851 /* Gain: '<S29>/Gain1' */
paulcox 0:70d27fec6d71 852 Gain1 = 0.866025388F * alpha_voltage;
paulcox 0:70d27fec6d71 853
paulcox 0:70d27fec6d71 854 /* Gain: '<S30>/Space_Vector_Gain' incorporates:
paulcox 0:70d27fec6d71 855 * Gain: '<S30>/Alpha_Gain'
paulcox 0:70d27fec6d71 856 * Sum: '<S30>/Add'
paulcox 0:70d27fec6d71 857 */
paulcox 0:70d27fec6d71 858 electrical_angle = (((1.73205078F * alpha_voltage) + 33.941124F) +
paulcox 0:70d27fec6d71 859 SignPreIntegrator_f) * 0.353553385F;
paulcox 0:70d27fec6d71 860
paulcox 0:70d27fec6d71 861 /* Gain: '<S33>/Va_Gain' incorporates:
paulcox 0:70d27fec6d71 862 * Gain: '<S33>/Alpha_Gain'
paulcox 0:70d27fec6d71 863 * Gain: '<S33>/Beta_Gain'
paulcox 0:70d27fec6d71 864 * Sum: '<S33>/Add'
paulcox 0:70d27fec6d71 865 */
paulcox 0:70d27fec6d71 866 Switch_fr = ((33.941124F - (1.73205078F * alpha_voltage)) + (3.0F *
paulcox 0:70d27fec6d71 867 SignPreIntegrator_f)) * 0.353553385F;
paulcox 0:70d27fec6d71 868
paulcox 0:70d27fec6d71 869 /* Gain: '<S36>/Space_Vector_Gain' incorporates:
paulcox 0:70d27fec6d71 870 * Gain: '<S36>/Alpha_Gain'
paulcox 0:70d27fec6d71 871 * Sum: '<S36>/Add'
paulcox 0:70d27fec6d71 872 */
paulcox 0:70d27fec6d71 873 cos_coefficient = ((33.941124F - (1.73205078F * alpha_voltage)) -
paulcox 0:70d27fec6d71 874 SignPreIntegrator_f) * 0.353553385F;
paulcox 0:70d27fec6d71 875
paulcox 0:70d27fec6d71 876 /* Gain: '<S31>/Space_Vector_Gain' incorporates:
paulcox 0:70d27fec6d71 877 * Constant: '<S27>/Bus_Voltage'
paulcox 0:70d27fec6d71 878 * Gain: '<S31>/Alpha_Gain'
paulcox 0:70d27fec6d71 879 * Sum: '<S31>/Add'
paulcox 0:70d27fec6d71 880 */
paulcox 0:70d27fec6d71 881 sin_coefficient = ((2.44948983F * alpha_voltage) + 24.0F) * 0.5F;
paulcox 0:70d27fec6d71 882
paulcox 0:70d27fec6d71 883 /* Gain: '<S34>/Va_Gain' incorporates:
paulcox 0:70d27fec6d71 884 * Constant: '<S27>/Bus_Voltage'
paulcox 0:70d27fec6d71 885 * Gain: '<S34>/Beta_Gain'
paulcox 0:70d27fec6d71 886 * Sum: '<S34>/Add'
paulcox 0:70d27fec6d71 887 */
paulcox 0:70d27fec6d71 888 Sectors_2_and_5_idx_1 = ((1.41421354F * SignPreIntegrator_f) + 24.0F) * 0.5F;
paulcox 0:70d27fec6d71 889
paulcox 0:70d27fec6d71 890 /* Gain: '<S37>/Space_Vector_Gain' incorporates:
paulcox 0:70d27fec6d71 891 * Constant: '<S27>/Bus_Voltage'
paulcox 0:70d27fec6d71 892 * Gain: '<S37>/Beta_Gain'
paulcox 0:70d27fec6d71 893 * Sum: '<S37>/Add'
paulcox 0:70d27fec6d71 894 */
paulcox 0:70d27fec6d71 895 Sectors_2_and_5_idx_2 = (24.0F - (1.41421354F * SignPreIntegrator_f)) * 0.5F;
paulcox 0:70d27fec6d71 896
paulcox 0:70d27fec6d71 897 /* Gain: '<S32>/Space_Vector_Gain' incorporates:
paulcox 0:70d27fec6d71 898 * Gain: '<S32>/Alpha_Gain'
paulcox 0:70d27fec6d71 899 * Sum: '<S32>/Add'
paulcox 0:70d27fec6d71 900 */
paulcox 0:70d27fec6d71 901 phase_voltages[0] = (((1.73205078F * alpha_voltage) + 33.941124F) -
paulcox 0:70d27fec6d71 902 SignPreIntegrator_f) * 0.353553385F;
paulcox 0:70d27fec6d71 903
paulcox 0:70d27fec6d71 904 /* Gain: '<S35>/Va_Gain' incorporates:
paulcox 0:70d27fec6d71 905 * Gain: '<S35>/Alpha_Gain'
paulcox 0:70d27fec6d71 906 * Sum: '<S35>/Add'
paulcox 0:70d27fec6d71 907 */
paulcox 0:70d27fec6d71 908 phase_voltages[1] = ((33.941124F - (1.73205078F * alpha_voltage)) +
paulcox 0:70d27fec6d71 909 SignPreIntegrator_f) * 0.353553385F;
paulcox 0:70d27fec6d71 910
paulcox 0:70d27fec6d71 911 /* Gain: '<S38>/Space_Vector_Gain' incorporates:
paulcox 0:70d27fec6d71 912 * Gain: '<S38>/Alpha_Gain'
paulcox 0:70d27fec6d71 913 * Gain: '<S38>/Beta_Gain'
paulcox 0:70d27fec6d71 914 * Sum: '<S38>/Add'
paulcox 0:70d27fec6d71 915 */
paulcox 0:70d27fec6d71 916 phase_voltages[2] = ((33.941124F - (1.73205078F * alpha_voltage)) - (3.0F *
paulcox 0:70d27fec6d71 917 SignPreIntegrator_f)) * 0.353553385F;
paulcox 0:70d27fec6d71 918
paulcox 0:70d27fec6d71 919 /* LookupNDDirect: '<S28>/Lookup_Table' incorporates:
paulcox 0:70d27fec6d71 920 * Constant: '<S39>/Constant'
paulcox 0:70d27fec6d71 921 * Constant: '<S40>/Constant'
paulcox 0:70d27fec6d71 922 * Constant: '<S41>/Constant'
paulcox 0:70d27fec6d71 923 * Gain: '<S28>/Sector_Gain_VB'
paulcox 0:70d27fec6d71 924 * Gain: '<S28>/Sector_Gain_VC'
paulcox 0:70d27fec6d71 925 * RelationalOperator: '<S39>/Compare'
paulcox 0:70d27fec6d71 926 * RelationalOperator: '<S40>/Compare'
paulcox 0:70d27fec6d71 927 * RelationalOperator: '<S41>/Compare'
paulcox 0:70d27fec6d71 928 * Sum: '<S28>/Calculate_Phase_Advanced_Sector'
paulcox 0:70d27fec6d71 929 * Sum: '<S29>/Add'
paulcox 0:70d27fec6d71 930 * Sum: '<S29>/Add1'
paulcox 0:70d27fec6d71 931 *
paulcox 0:70d27fec6d71 932 * About '<S28>/Lookup_Table':
paulcox 0:70d27fec6d71 933 * 1-dimensional Direct Look-Up returning a Scalar
paulcox 0:70d27fec6d71 934 */
paulcox 0:70d27fec6d71 935 u0 = (int16_T)(((((Gain1 - IntegralGain_j) > 0.0F) << 1) +
paulcox 0:70d27fec6d71 936 (SignPreIntegrator_f > 0.0F)) + ((((0.0F - IntegralGain_j) -
paulcox 0:70d27fec6d71 937 Gain1) > 0.0F) << 2));
paulcox 0:70d27fec6d71 938 if (u0 > 6) {
paulcox 0:70d27fec6d71 939 u0 = 6;
paulcox 0:70d27fec6d71 940 }
paulcox 0:70d27fec6d71 941
paulcox 0:70d27fec6d71 942 /* MultiPortSwitch: '<S27>/Select_Sector' incorporates:
paulcox 0:70d27fec6d71 943 * LookupNDDirect: '<S28>/Lookup_Table'
paulcox 0:70d27fec6d71 944 *
paulcox 0:70d27fec6d71 945 * About '<S28>/Lookup_Table':
paulcox 0:70d27fec6d71 946 * 1-dimensional Direct Look-Up returning a Scalar
paulcox 0:70d27fec6d71 947 */
paulcox 0:70d27fec6d71 948 switch (ConstP.Lookup_Table_table[u0]) {
paulcox 0:70d27fec6d71 949 case 1:
paulcox 0:70d27fec6d71 950 phase_voltages[0] = electrical_angle;
paulcox 0:70d27fec6d71 951 phase_voltages[1] = Switch_fr;
paulcox 0:70d27fec6d71 952 phase_voltages[2] = cos_coefficient;
paulcox 0:70d27fec6d71 953 break;
paulcox 0:70d27fec6d71 954
paulcox 0:70d27fec6d71 955 case 2:
paulcox 0:70d27fec6d71 956 phase_voltages[0] = sin_coefficient;
paulcox 0:70d27fec6d71 957 phase_voltages[1] = Sectors_2_and_5_idx_1;
paulcox 0:70d27fec6d71 958 phase_voltages[2] = Sectors_2_and_5_idx_2;
paulcox 0:70d27fec6d71 959 break;
paulcox 0:70d27fec6d71 960
paulcox 0:70d27fec6d71 961 case 3:
paulcox 0:70d27fec6d71 962 break;
paulcox 0:70d27fec6d71 963
paulcox 0:70d27fec6d71 964 case 4:
paulcox 0:70d27fec6d71 965 phase_voltages[0] = electrical_angle;
paulcox 0:70d27fec6d71 966 phase_voltages[1] = Switch_fr;
paulcox 0:70d27fec6d71 967 phase_voltages[2] = cos_coefficient;
paulcox 0:70d27fec6d71 968 break;
paulcox 0:70d27fec6d71 969
paulcox 0:70d27fec6d71 970 case 5:
paulcox 0:70d27fec6d71 971 phase_voltages[0] = sin_coefficient;
paulcox 0:70d27fec6d71 972 phase_voltages[1] = Sectors_2_and_5_idx_1;
paulcox 0:70d27fec6d71 973 phase_voltages[2] = Sectors_2_and_5_idx_2;
paulcox 0:70d27fec6d71 974 break;
paulcox 0:70d27fec6d71 975 }
paulcox 0:70d27fec6d71 976
paulcox 0:70d27fec6d71 977 /* End of MultiPortSwitch: '<S27>/Select_Sector' */
paulcox 0:70d27fec6d71 978
paulcox 0:70d27fec6d71 979 /* Switch: '<S6>/Switch' */
paulcox 0:70d27fec6d71 980 if (DWork.Lo_to_Hi_Rate_Transition3_Buffe) {
paulcox 0:70d27fec6d71 981 /* Outport: '<Root>/pwm_compare' */
paulcox 0:70d27fec6d71 982 pwm_compare[0] = 1500U;
paulcox 0:70d27fec6d71 983 pwm_compare[1] = 1500U;
paulcox 0:70d27fec6d71 984 pwm_compare[2] = 1500U;
paulcox 0:70d27fec6d71 985 } else {
paulcox 0:70d27fec6d71 986 /* Gain: '<S6>/Voltage to PWM Compare Units' */
paulcox 0:70d27fec6d71 987 electrical_angle = 125.0F * phase_voltages[0];
paulcox 0:70d27fec6d71 988
paulcox 0:70d27fec6d71 989 /* Saturate: '<S6>/Saturation' */
paulcox 0:70d27fec6d71 990 if (electrical_angle > 2999.0F) {
paulcox 0:70d27fec6d71 991 /* Outport: '<Root>/pwm_compare' incorporates:
paulcox 0:70d27fec6d71 992 * DataTypeConversion: '<S6>/Quantize'
paulcox 0:70d27fec6d71 993 */
paulcox 0:70d27fec6d71 994 pwm_compare[0] = 2999U;
paulcox 0:70d27fec6d71 995 } else if (electrical_angle < 0.0F) {
paulcox 0:70d27fec6d71 996 /* Outport: '<Root>/pwm_compare' incorporates:
paulcox 0:70d27fec6d71 997 * DataTypeConversion: '<S6>/Quantize'
paulcox 0:70d27fec6d71 998 */
paulcox 0:70d27fec6d71 999 pwm_compare[0] = 0U;
paulcox 0:70d27fec6d71 1000 } else {
paulcox 0:70d27fec6d71 1001 /* Outport: '<Root>/pwm_compare' incorporates:
paulcox 0:70d27fec6d71 1002 * DataTypeConversion: '<S6>/Quantize'
paulcox 0:70d27fec6d71 1003 */
paulcox 0:70d27fec6d71 1004 pwm_compare[0] = (uint16_T)electrical_angle;
paulcox 0:70d27fec6d71 1005 }
paulcox 0:70d27fec6d71 1006
paulcox 0:70d27fec6d71 1007 /* Gain: '<S6>/Voltage to PWM Compare Units' */
paulcox 0:70d27fec6d71 1008 electrical_angle = 125.0F * phase_voltages[1];
paulcox 0:70d27fec6d71 1009
paulcox 0:70d27fec6d71 1010 /* Saturate: '<S6>/Saturation' */
paulcox 0:70d27fec6d71 1011 if (electrical_angle > 2999.0F) {
paulcox 0:70d27fec6d71 1012 /* Outport: '<Root>/pwm_compare' incorporates:
paulcox 0:70d27fec6d71 1013 * DataTypeConversion: '<S6>/Quantize'
paulcox 0:70d27fec6d71 1014 */
paulcox 0:70d27fec6d71 1015 pwm_compare[1] = 2999U;
paulcox 0:70d27fec6d71 1016 } else if (electrical_angle < 0.0F) {
paulcox 0:70d27fec6d71 1017 /* Outport: '<Root>/pwm_compare' incorporates:
paulcox 0:70d27fec6d71 1018 * DataTypeConversion: '<S6>/Quantize'
paulcox 0:70d27fec6d71 1019 */
paulcox 0:70d27fec6d71 1020 pwm_compare[1] = 0U;
paulcox 0:70d27fec6d71 1021 } else {
paulcox 0:70d27fec6d71 1022 /* Outport: '<Root>/pwm_compare' incorporates:
paulcox 0:70d27fec6d71 1023 * DataTypeConversion: '<S6>/Quantize'
paulcox 0:70d27fec6d71 1024 */
paulcox 0:70d27fec6d71 1025 pwm_compare[1] = (uint16_T)electrical_angle;
paulcox 0:70d27fec6d71 1026 }
paulcox 0:70d27fec6d71 1027
paulcox 0:70d27fec6d71 1028 /* Gain: '<S6>/Voltage to PWM Compare Units' */
paulcox 0:70d27fec6d71 1029 electrical_angle = 125.0F * phase_voltages[2];
paulcox 0:70d27fec6d71 1030
paulcox 0:70d27fec6d71 1031 /* Saturate: '<S6>/Saturation' */
paulcox 0:70d27fec6d71 1032 if (electrical_angle > 2999.0F) {
paulcox 0:70d27fec6d71 1033 /* Outport: '<Root>/pwm_compare' incorporates:
paulcox 0:70d27fec6d71 1034 * DataTypeConversion: '<S6>/Quantize'
paulcox 0:70d27fec6d71 1035 */
paulcox 0:70d27fec6d71 1036 pwm_compare[2] = 2999U;
paulcox 0:70d27fec6d71 1037 } else if (electrical_angle < 0.0F) {
paulcox 0:70d27fec6d71 1038 /* Outport: '<Root>/pwm_compare' incorporates:
paulcox 0:70d27fec6d71 1039 * DataTypeConversion: '<S6>/Quantize'
paulcox 0:70d27fec6d71 1040 */
paulcox 0:70d27fec6d71 1041 pwm_compare[2] = 0U;
paulcox 0:70d27fec6d71 1042 } else {
paulcox 0:70d27fec6d71 1043 /* Outport: '<Root>/pwm_compare' incorporates:
paulcox 0:70d27fec6d71 1044 * DataTypeConversion: '<S6>/Quantize'
paulcox 0:70d27fec6d71 1045 */
paulcox 0:70d27fec6d71 1046 pwm_compare[2] = (uint16_T)electrical_angle;
paulcox 0:70d27fec6d71 1047 }
paulcox 0:70d27fec6d71 1048 }
paulcox 0:70d27fec6d71 1049
paulcox 0:70d27fec6d71 1050 /* End of Switch: '<S6>/Switch' */
paulcox 0:70d27fec6d71 1051
paulcox 0:70d27fec6d71 1052 /* DeadZone: '<S19>/DeadZone' */
paulcox 0:70d27fec6d71 1053 if (SignDeltaU > 12.0F) {
paulcox 0:70d27fec6d71 1054 SignDeltaU -= 12.0F;
paulcox 0:70d27fec6d71 1055 } else if (SignDeltaU >= -12.0F) {
paulcox 0:70d27fec6d71 1056 SignDeltaU = 0.0F;
paulcox 0:70d27fec6d71 1057 } else {
paulcox 0:70d27fec6d71 1058 SignDeltaU -= -12.0F;
paulcox 0:70d27fec6d71 1059 }
paulcox 0:70d27fec6d71 1060
paulcox 0:70d27fec6d71 1061 /* End of DeadZone: '<S19>/DeadZone' */
paulcox 0:70d27fec6d71 1062
paulcox 0:70d27fec6d71 1063 /* RelationalOperator: '<S19>/NotEqual' */
paulcox 0:70d27fec6d71 1064 NotEqual_b = (0.0F != SignDeltaU);
paulcox 0:70d27fec6d71 1065
paulcox 0:70d27fec6d71 1066 /* Signum: '<S19>/SignDeltaU' */
paulcox 0:70d27fec6d71 1067 if (SignDeltaU < 0.0F) {
paulcox 0:70d27fec6d71 1068 SignDeltaU = -1.0F;
paulcox 0:70d27fec6d71 1069 } else {
paulcox 0:70d27fec6d71 1070 if (SignDeltaU > 0.0F) {
paulcox 0:70d27fec6d71 1071 SignDeltaU = 1.0F;
paulcox 0:70d27fec6d71 1072 }
paulcox 0:70d27fec6d71 1073 }
paulcox 0:70d27fec6d71 1074
paulcox 0:70d27fec6d71 1075 /* End of Signum: '<S19>/SignDeltaU' */
paulcox 0:70d27fec6d71 1076
paulcox 0:70d27fec6d71 1077 /* Gain: '<S17>/Integral Gain' */
paulcox 0:70d27fec6d71 1078 IntegralGain_j = ctrlParams.Current_I * d_current_error;
paulcox 0:70d27fec6d71 1079
paulcox 0:70d27fec6d71 1080 /* DataTypeConversion: '<S19>/DataTypeConv1' */
paulcox 0:70d27fec6d71 1081 if (SignDeltaU < 128.0F) {
paulcox 0:70d27fec6d71 1082 rtPrevAction = (int8_T)SignDeltaU;
paulcox 0:70d27fec6d71 1083 } else {
paulcox 0:70d27fec6d71 1084 rtPrevAction = MAX_int8_T;
paulcox 0:70d27fec6d71 1085 }
paulcox 0:70d27fec6d71 1086
paulcox 0:70d27fec6d71 1087 /* End of DataTypeConversion: '<S19>/DataTypeConv1' */
paulcox 0:70d27fec6d71 1088
paulcox 0:70d27fec6d71 1089 /* Signum: '<S19>/SignPreIntegrator' */
paulcox 0:70d27fec6d71 1090 if (IntegralGain_j < 0.0F) {
paulcox 0:70d27fec6d71 1091 electrical_angle = -1.0F;
paulcox 0:70d27fec6d71 1092 } else if (IntegralGain_j > 0.0F) {
paulcox 0:70d27fec6d71 1093 electrical_angle = 1.0F;
paulcox 0:70d27fec6d71 1094 } else {
paulcox 0:70d27fec6d71 1095 electrical_angle = IntegralGain_j;
paulcox 0:70d27fec6d71 1096 }
paulcox 0:70d27fec6d71 1097
paulcox 0:70d27fec6d71 1098 /* Switch: '<S17>/Switch' incorporates:
paulcox 0:70d27fec6d71 1099 * Constant: '<S17>/Constant'
paulcox 0:70d27fec6d71 1100 * DataTypeConversion: '<S19>/DataTypeConv2'
paulcox 0:70d27fec6d71 1101 * Logic: '<S19>/AND'
paulcox 0:70d27fec6d71 1102 * RelationalOperator: '<S19>/Equal'
paulcox 0:70d27fec6d71 1103 * Signum: '<S19>/SignPreIntegrator'
paulcox 0:70d27fec6d71 1104 */
paulcox 0:70d27fec6d71 1105 if (NotEqual_b && (rtPrevAction == ((int8_T)electrical_angle))) {
paulcox 0:70d27fec6d71 1106 electrical_angle = 0.0F;
paulcox 0:70d27fec6d71 1107 } else {
paulcox 0:70d27fec6d71 1108 electrical_angle = IntegralGain_j;
paulcox 0:70d27fec6d71 1109 }
paulcox 0:70d27fec6d71 1110
paulcox 0:70d27fec6d71 1111 /* End of Switch: '<S17>/Switch' */
paulcox 0:70d27fec6d71 1112
paulcox 0:70d27fec6d71 1113 /* DeadZone: '<S20>/DeadZone' */
paulcox 0:70d27fec6d71 1114 if (SignDeltaU_b > 12.0F) {
paulcox 0:70d27fec6d71 1115 SignDeltaU_b -= 12.0F;
paulcox 0:70d27fec6d71 1116 } else if (SignDeltaU_b >= -12.0F) {
paulcox 0:70d27fec6d71 1117 SignDeltaU_b = 0.0F;
paulcox 0:70d27fec6d71 1118 } else {
paulcox 0:70d27fec6d71 1119 SignDeltaU_b -= -12.0F;
paulcox 0:70d27fec6d71 1120 }
paulcox 0:70d27fec6d71 1121
paulcox 0:70d27fec6d71 1122 /* End of DeadZone: '<S20>/DeadZone' */
paulcox 0:70d27fec6d71 1123
paulcox 0:70d27fec6d71 1124 /* RelationalOperator: '<S20>/NotEqual' */
paulcox 0:70d27fec6d71 1125 NotEqual_b = (0.0F != SignDeltaU_b);
paulcox 0:70d27fec6d71 1126
paulcox 0:70d27fec6d71 1127 /* Signum: '<S20>/SignDeltaU' */
paulcox 0:70d27fec6d71 1128 if (SignDeltaU_b < 0.0F) {
paulcox 0:70d27fec6d71 1129 SignDeltaU_b = -1.0F;
paulcox 0:70d27fec6d71 1130 } else {
paulcox 0:70d27fec6d71 1131 if (SignDeltaU_b > 0.0F) {
paulcox 0:70d27fec6d71 1132 SignDeltaU_b = 1.0F;
paulcox 0:70d27fec6d71 1133 }
paulcox 0:70d27fec6d71 1134 }
paulcox 0:70d27fec6d71 1135
paulcox 0:70d27fec6d71 1136 /* End of Signum: '<S20>/SignDeltaU' */
paulcox 0:70d27fec6d71 1137
paulcox 0:70d27fec6d71 1138 /* Gain: '<S18>/Integral Gain' */
paulcox 0:70d27fec6d71 1139 IntegralGain_j = ctrlParams.Current_I * q_current_error;
paulcox 0:70d27fec6d71 1140 if (M->Timing.TaskCounters.TID[1] == 0) {
paulcox 0:70d27fec6d71 1141 /* RelationalOperator: '<S6>/Relational Operator' incorporates:
paulcox 0:70d27fec6d71 1142 * Constant: '<S47>/Constant'
paulcox 0:70d27fec6d71 1143 */
paulcox 0:70d27fec6d71 1144 RelationalOperator = (controller_mode == StandBy);
paulcox 0:70d27fec6d71 1145
paulcox 0:70d27fec6d71 1146 /* Outputs for Enabled SubSystem: '<S58>/Generate_Error' incorporates:
paulcox 0:70d27fec6d71 1147 * EnablePort: '<S60>/Enable'
paulcox 0:70d27fec6d71 1148 */
paulcox 0:70d27fec6d71 1149 /* Logic: '<S58>/AND' incorporates:
paulcox 0:70d27fec6d71 1150 * Abs: '<S58>/Velocity_Abs'
paulcox 0:70d27fec6d71 1151 * Constant: '<S58>/Max_Valid_Velocity_Change'
paulcox 0:70d27fec6d71 1152 * Constant: '<S59>/Constant'
paulcox 0:70d27fec6d71 1153 * Delay: '<S58>/Velocity_Delay'
paulcox 0:70d27fec6d71 1154 * RelationalOperator: '<S58>/Excessive_Velocity_Change'
paulcox 0:70d27fec6d71 1155 * RelationalOperator: '<S58>/Relational_Operator'
paulcox 0:70d27fec6d71 1156 * Sum: '<S58>/Velocity_Difference'
paulcox 0:70d27fec6d71 1157 */
paulcox 0:70d27fec6d71 1158 if ((controller_mode == VelocityControl) && (((real32_T)fabs
paulcox 0:70d27fec6d71 1159 (velocity_measured - DWork.Velocity_Delay_DSTATE)) >= 628.318542F)) {
paulcox 0:70d27fec6d71 1160 /* DataStoreWrite: '<S60>/Data_Store_Write' incorporates:
paulcox 0:70d27fec6d71 1161 * Constant: '<S61>/Constant'
paulcox 0:70d27fec6d71 1162 */
paulcox 0:70d27fec6d71 1163 DWork.error_l = MeasuredVelocityError;
paulcox 0:70d27fec6d71 1164 }
paulcox 0:70d27fec6d71 1165
paulcox 0:70d27fec6d71 1166 /* End of Logic: '<S58>/AND' */
paulcox 0:70d27fec6d71 1167 /* End of Outputs for SubSystem: '<S58>/Generate_Error' */
paulcox 0:70d27fec6d71 1168 }
paulcox 0:70d27fec6d71 1169
paulcox 0:70d27fec6d71 1170 /* Outport: '<Root>/error' incorporates:
paulcox 0:70d27fec6d71 1171 * DataStoreRead: '<Root>/Data Store Read'
paulcox 0:70d27fec6d71 1172 */
paulcox 0:70d27fec6d71 1173 error = DWork.error_l;
paulcox 0:70d27fec6d71 1174
paulcox 0:70d27fec6d71 1175 /* Update for RateTransition: '<S6>/Lo_to_Hi_Rate_Transition3' */
paulcox 0:70d27fec6d71 1176 if (M->Timing.TaskCounters.TID[1] == 0) {
paulcox 0:70d27fec6d71 1177 DWork.Lo_to_Hi_Rate_Transition3_Buffe = RelationalOperator;
paulcox 0:70d27fec6d71 1178
paulcox 0:70d27fec6d71 1179 /* Update for Delay: '<S55>/Position_Delay' */
paulcox 0:70d27fec6d71 1180 DWork.Position_Delay_DSTATE = Wrap_To_Pi;
paulcox 0:70d27fec6d71 1181 }
paulcox 0:70d27fec6d71 1182
paulcox 0:70d27fec6d71 1183 /* End of Update for RateTransition: '<S6>/Lo_to_Hi_Rate_Transition3' */
paulcox 0:70d27fec6d71 1184
paulcox 0:70d27fec6d71 1185 /* Update for UnitDelay: '<S13>/Delay Input1' */
paulcox 0:70d27fec6d71 1186 DWork.DelayInput1_DSTATE = Enum_To_Int;
paulcox 0:70d27fec6d71 1187
paulcox 0:70d27fec6d71 1188 /* Update for DiscreteIntegrator: '<S17>/Integrator' */
paulcox 0:70d27fec6d71 1189 if (FixPtRelationalOperator == 0) {
paulcox 0:70d27fec6d71 1190 DWork.Integrator_DSTATE += 4.0E-5F * electrical_angle;
paulcox 0:70d27fec6d71 1191 }
paulcox 0:70d27fec6d71 1192
paulcox 0:70d27fec6d71 1193 if (FixPtRelationalOperator > 0) {
paulcox 0:70d27fec6d71 1194 DWork.Integrator_PrevResetState = 1;
paulcox 0:70d27fec6d71 1195 } else {
paulcox 0:70d27fec6d71 1196 DWork.Integrator_PrevResetState = 0;
paulcox 0:70d27fec6d71 1197 }
paulcox 0:70d27fec6d71 1198
paulcox 0:70d27fec6d71 1199 /* End of Update for DiscreteIntegrator: '<S17>/Integrator' */
paulcox 0:70d27fec6d71 1200
paulcox 0:70d27fec6d71 1201 /* Update for DiscreteIntegrator: '<S18>/Integrator' */
paulcox 0:70d27fec6d71 1202 if (FixPtRelationalOperator == 0) {
paulcox 0:70d27fec6d71 1203 /* DataTypeConversion: '<S20>/DataTypeConv1' */
paulcox 0:70d27fec6d71 1204 if (SignDeltaU_b < 128.0F) {
paulcox 0:70d27fec6d71 1205 rtPrevAction = (int8_T)SignDeltaU_b;
paulcox 0:70d27fec6d71 1206 } else {
paulcox 0:70d27fec6d71 1207 rtPrevAction = MAX_int8_T;
paulcox 0:70d27fec6d71 1208 }
paulcox 0:70d27fec6d71 1209
paulcox 0:70d27fec6d71 1210 /* End of DataTypeConversion: '<S20>/DataTypeConv1' */
paulcox 0:70d27fec6d71 1211
paulcox 0:70d27fec6d71 1212 /* Signum: '<S20>/SignPreIntegrator' */
paulcox 0:70d27fec6d71 1213 if (IntegralGain_j < 0.0F) {
paulcox 0:70d27fec6d71 1214 electrical_angle = -1.0F;
paulcox 0:70d27fec6d71 1215 } else if (IntegralGain_j > 0.0F) {
paulcox 0:70d27fec6d71 1216 electrical_angle = 1.0F;
paulcox 0:70d27fec6d71 1217 } else {
paulcox 0:70d27fec6d71 1218 electrical_angle = IntegralGain_j;
paulcox 0:70d27fec6d71 1219 }
paulcox 0:70d27fec6d71 1220
paulcox 0:70d27fec6d71 1221 /* Switch: '<S18>/Switch' incorporates:
paulcox 0:70d27fec6d71 1222 * Constant: '<S18>/Constant'
paulcox 0:70d27fec6d71 1223 * DataTypeConversion: '<S20>/DataTypeConv2'
paulcox 0:70d27fec6d71 1224 * Logic: '<S20>/AND'
paulcox 0:70d27fec6d71 1225 * RelationalOperator: '<S20>/Equal'
paulcox 0:70d27fec6d71 1226 * Signum: '<S20>/SignPreIntegrator'
paulcox 0:70d27fec6d71 1227 */
paulcox 0:70d27fec6d71 1228 if (NotEqual_b && (rtPrevAction == ((int8_T)electrical_angle))) {
paulcox 0:70d27fec6d71 1229 IntegralGain_j = 0.0F;
paulcox 0:70d27fec6d71 1230 }
paulcox 0:70d27fec6d71 1231
paulcox 0:70d27fec6d71 1232 /* End of Switch: '<S18>/Switch' */
paulcox 0:70d27fec6d71 1233 DWork.Integrator_DSTATE_l += 4.0E-5F * IntegralGain_j;
paulcox 0:70d27fec6d71 1234 }
paulcox 0:70d27fec6d71 1235
paulcox 0:70d27fec6d71 1236 if (FixPtRelationalOperator > 0) {
paulcox 0:70d27fec6d71 1237 DWork.Integrator_PrevResetState_c = 1;
paulcox 0:70d27fec6d71 1238 } else {
paulcox 0:70d27fec6d71 1239 DWork.Integrator_PrevResetState_c = 0;
paulcox 0:70d27fec6d71 1240 }
paulcox 0:70d27fec6d71 1241
paulcox 0:70d27fec6d71 1242 /* End of Update for DiscreteIntegrator: '<S18>/Integrator' */
paulcox 0:70d27fec6d71 1243 if (M->Timing.TaskCounters.TID[1] == 0) {
paulcox 0:70d27fec6d71 1244 /* Update for Delay: '<S58>/Velocity_Delay' */
paulcox 0:70d27fec6d71 1245 DWork.Velocity_Delay_DSTATE = velocity_measured;
paulcox 0:70d27fec6d71 1246 }
paulcox 0:70d27fec6d71 1247
paulcox 0:70d27fec6d71 1248 rate_scheduler();
paulcox 0:70d27fec6d71 1249 return error;
paulcox 0:70d27fec6d71 1250 }
paulcox 0:70d27fec6d71 1251
paulcox 0:70d27fec6d71 1252 /* Model initialize function */
paulcox 0:70d27fec6d71 1253 extern "C" void Controller_Init(void)
paulcox 0:70d27fec6d71 1254 {
paulcox 0:70d27fec6d71 1255 /* Registration code */
paulcox 0:70d27fec6d71 1256
paulcox 0:70d27fec6d71 1257 /* initialize real-time model */
paulcox 0:70d27fec6d71 1258 (void) memset((void *)M, 0,
paulcox 0:70d27fec6d71 1259 sizeof(RT_MODEL));
paulcox 0:70d27fec6d71 1260
paulcox 0:70d27fec6d71 1261 /* block I/O */
paulcox 0:70d27fec6d71 1262
paulcox 0:70d27fec6d71 1263 /* exported global signals */
paulcox 0:70d27fec6d71 1264 phase_currents[0] = 0.0F;
paulcox 0:70d27fec6d71 1265 phase_currents[1] = 0.0F;
paulcox 0:70d27fec6d71 1266 rotor_position = 0.0F;
paulcox 0:70d27fec6d71 1267 velocity_measured = 0.0F;
paulcox 0:70d27fec6d71 1268 d_current_error = 0.0F;
paulcox 0:70d27fec6d71 1269 q_current_command = 0.0F;
paulcox 0:70d27fec6d71 1270 q_current_measured = 0.0F;
paulcox 0:70d27fec6d71 1271 q_current_error = 0.0F;
paulcox 0:70d27fec6d71 1272 phase_voltages[0] = 0.0F;
paulcox 0:70d27fec6d71 1273 phase_voltages[1] = 0.0F;
paulcox 0:70d27fec6d71 1274 phase_voltages[2] = 0.0F;
paulcox 0:70d27fec6d71 1275 velocity_error = 0.0F;
paulcox 0:70d27fec6d71 1276 controller_mode = StandBy;
paulcox 0:70d27fec6d71 1277
paulcox 0:70d27fec6d71 1278 /* states (dwork) */
paulcox 0:70d27fec6d71 1279 (void) memset((void *)&DWork, 0,
paulcox 0:70d27fec6d71 1280 sizeof(D_Work));
paulcox 0:70d27fec6d71 1281
paulcox 0:70d27fec6d71 1282 /* InitializeConditions for Enabled SubSystem: '<S51>/Open Loop Position' */
paulcox 0:70d27fec6d71 1283 /* InitializeConditions for DiscreteIntegrator: '<S63>/Integrate_To_Position' */
paulcox 0:70d27fec6d71 1284 DWork.Integrate_To_Position_DSTATE = 0.0F;
paulcox 0:70d27fec6d71 1285
paulcox 0:70d27fec6d71 1286 /* InitializeConditions for DiscreteIntegrator: '<S63>/Integrate_To_Velocity' */
paulcox 0:70d27fec6d71 1287 DWork.Integrate_To_Velocity_DSTATE = 0.0F;
paulcox 0:70d27fec6d71 1288
paulcox 0:70d27fec6d71 1289 /* End of InitializeConditions for SubSystem: '<S51>/Open Loop Position' */
paulcox 0:70d27fec6d71 1290
paulcox 0:70d27fec6d71 1291 /* Start for SwitchCase: '<S5>/Switch Case' */
paulcox 0:70d27fec6d71 1292 DWork.SwitchCase_ActiveSubsystem = -1;
paulcox 0:70d27fec6d71 1293
paulcox 0:70d27fec6d71 1294 /* InitializeConditions for IfAction SubSystem: '<S5>/Velocity_Control' */
paulcox 0:70d27fec6d71 1295 /* InitializeConditions for DiscreteIntegrator: '<S45>/Integrator' */
paulcox 0:70d27fec6d71 1296 DWork.Integrator_DSTATE_f = 0.0F;
paulcox 0:70d27fec6d71 1297
paulcox 0:70d27fec6d71 1298 /* End of InitializeConditions for SubSystem: '<S5>/Velocity_Control' */
paulcox 0:70d27fec6d71 1299
paulcox 0:70d27fec6d71 1300 /* InitializeConditions for IfAction SubSystem: '<S5>/Position_Control' */
paulcox 0:70d27fec6d71 1301 /* InitializeConditions for DiscreteIntegrator: '<S42>/Integrator' */
paulcox 0:70d27fec6d71 1302 DWork.Integrator_DSTATE_lc = 0.0F;
paulcox 0:70d27fec6d71 1303
paulcox 0:70d27fec6d71 1304 /* End of InitializeConditions for SubSystem: '<S5>/Position_Control' */
paulcox 0:70d27fec6d71 1305
paulcox 0:70d27fec6d71 1306 /* InitializeConditions for Chart: '<S49>/Wait_For_Valid_Position' */
paulcox 0:70d27fec6d71 1307 DWork.temporalCounter_i1 = 0U;
paulcox 0:70d27fec6d71 1308 DWork.is_active_c2_rtwdemo_pmsmfoc = 0U;
paulcox 0:70d27fec6d71 1309 DWork.is_c2_rtwdemo_pmsmfoc = IN_NO_ACTIVE_CHILD;
paulcox 0:70d27fec6d71 1310 DWork.Position_Valid = 0U;
paulcox 0:70d27fec6d71 1311
paulcox 0:70d27fec6d71 1312 /* InitializeConditions for Chart: '<S1>/Controller_Mode_Scheduler' */
paulcox 0:70d27fec6d71 1313 DWork.is_Motor_On = IN_NO_ACTIVE_CHILD;
paulcox 0:70d27fec6d71 1314 DWork.is_Motor_Control = IN_NO_ACTIVE_CHILD;
paulcox 0:70d27fec6d71 1315 DWork.velocity_command = 0.0F;
paulcox 0:70d27fec6d71 1316 DWork.position_command = 0.0F;
paulcox 0:70d27fec6d71 1317 DWork.torque_command = 0.0F;
paulcox 0:70d27fec6d71 1318
paulcox 0:70d27fec6d71 1319 /* Entry: Mode_Scheduler/Controller_Mode_Scheduler */
paulcox 0:70d27fec6d71 1320 /* Entry Internal: Mode_Scheduler/Controller_Mode_Scheduler */
paulcox 0:70d27fec6d71 1321 /* Transition: '<S4>:9' */
paulcox 0:70d27fec6d71 1322 DWork.is_c1_rtwdemo_pmsmfoc = IN_Stand_By;
paulcox 0:70d27fec6d71 1323
paulcox 0:70d27fec6d71 1324 /* Entry 'Stand_By': '<S4>:154' */
paulcox 0:70d27fec6d71 1325 controller_mode = StandBy;
paulcox 0:70d27fec6d71 1326
paulcox 0:70d27fec6d71 1327 /* InitializeConditions for DiscreteIntegrator: '<S17>/Integrator' */
paulcox 0:70d27fec6d71 1328 DWork.Integrator_PrevResetState = 0;
paulcox 0:70d27fec6d71 1329
paulcox 0:70d27fec6d71 1330 /* InitializeConditions for DiscreteIntegrator: '<S18>/Integrator' */
paulcox 0:70d27fec6d71 1331 DWork.Integrator_PrevResetState_c = 0;
paulcox 0:70d27fec6d71 1332 }
paulcox 0:70d27fec6d71 1333
paulcox 0:70d27fec6d71 1334 /*
paulcox 0:70d27fec6d71 1335 * File trailer for generated code.
paulcox 0:70d27fec6d71 1336 *
paulcox 0:70d27fec6d71 1337 * [EOF]
paulcox 0:70d27fec6d71 1338 */