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