File: C:\Mariano\Temp\SLUG_Autopilot\Stable\controlMCUSlugsMKII_dspic\sources\controlMCUSlugsMKII.c 1 /* 2 * File: controlMCUSlugsMKII.c 3 * 4 * Real-Time Workshop code generated for Simulink model controlMCUSlugsMKII. 5 * 6 * Model version : 1.716 7 * Real-Time Workshop file version : 6.6 (R2007a) 01-Feb-2007 8 * Real-Time Workshop file generated on : Sat Nov 27 11:43:56 2010 9 * TLC version : 6.6 (Jan 16 2007) 10 * C source code generated on : Sat Nov 27 11:43:58 2010 11 *-------------------------------------------------------------- 12 * Embedded Coder for Microchip dsPIC family. | 13 * Generate .c and .h files from your Matlab/simulink model | 14 * and compile the diagram to .hex and .coff file that can be | 15 * downloaded directly into the microcontroller | 16 * | 17 * Licence Accorded to Mariano I. Lizarraga | 18 * University of California Santa Cruz (UCSC)| 19 * | 20 * Written by Lubin KERHUEL - http://www.kerhuel.eu | 21 * - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - | 22 * Version 0.99b+ 17-Mar-2009 | 23 * For Matlab 7.4 R2007a | 24 *-------------------------------------------------------------- 25 */ 26 27 #include "controlMCUSlugsMKII.h" 28 #include "controlMCUSlugsMKII_private.h" 29 30 /* Block signals (auto storage) */ 31 BlockIO_controlMCUSlugsMKII controlMCUSlugsMKII_B; 32 33 /* Block states (auto storage) */ 34 D_Work_controlMCUSlugsMKII controlMCUSlugsMKII_DWork; 35 36 /* Previous zero-crossings (trigger) states */ 37 PrevZCSigStates_controlMCUSlugs controlMCUSlugsM_PrevZCSigState; 38 39 /* Real-time model */ 40 RT_MODEL_controlMCUSlugsMKII controlMCUSlugsMKII_M_; 41 RT_MODEL_controlMCUSlugsMKII *controlMCUSlugsMKII_M = &controlMCUSlugsMKII_M_; 42 static void rate_monotonic_scheduler(void); 43 44 /* Set which subrates need to run this base step (base rate always runs). 45 * This function must be called prior to calling the model step function 46 * in order to "remember" which rates need to run this base step. The 47 * buffering of events allows for overlapping preemption. 48 */ 49 void controlMCUSlugsMKII_SetEventsForThisBaseStep(boolean_T *eventFlags) 50 { 51 /* Task runs when its counter is zero, computed via rtmStepTask macro */ 52 eventFlags[1] = ((boolean_T)rtmStepTask(controlMCUSlugsMKII_M, 1)); 53 } 54 55 /* This function updates active task flag for each subrate 56 * and rate transition flags for tasks that exchagne data. 57 * The function assumes rate-monotonic multitasking scheduler. 58 * The function must be called at model base rate so that 59 * the generated code self-manages all its subrates and rate 60 * transition flags. 61 */ 62 static void rate_monotonic_scheduler(void) 63 { 64 /* Compute which subrates run during the next base time step. Subrates 65 * are an integer multiple of the base rate counter. Therefore, the subtask 66 * counter is reset when it reaches its limit (zero means run). 67 */ 68 if (++controlMCUSlugsMKII_M->Timing.TaskCounters.TID[1] == 100) {/* Sample time: [1.0s, 0.0s] */ 69 controlMCUSlugsMKII_M->Timing.TaskCounters.TID[1] = 0; 70 } 71 } 72 73 /* Output and update for atomic system: 74 * '<S70>/negprotect' 75 * '<S113>/negprotect' 76 * '<S237>/negprotect' 77 * '<S239>/negprotect1' 78 */ 79 void controlMCUSlugsM_negprotect(real32_T rtu_val, 80 rtB_negprotect_controlMCUSlugsM *localB) 81 { 82 /* Embedded MATLAB: '<S70>/negprotect' */ 83 if (rtu_val >= 1.000000047E-003F) { 84 /* This block supports an embeddable subset of the MATLAB language. */ 85 /* See the help menu for details. */ 86 localB->zpVal = rtu_val; 87 } else { 88 localB->zpVal = 1.000000047E-003F; 89 } 90 } 91 92 /* Output and update for atomic system: 93 * '<S14>/myMux Fun1' 94 * '<S16>/myMux Fun1' 95 * '<S2>/myMux Fun2' 96 */ 97 void controlMCUSlugsMK_myMuxFun1(real32_T rtu_u1, real32_T rtu_u2, real32_T 98 rtu_u3, rtB_myMuxFun1_controlMCUSlugsMK *localB) 99 { 100 /* Embedded MATLAB: '<S14>/myMux Fun1' */ 101 /* This block supports an embeddable subset of the MATLAB language. */ 102 /* See the help menu for details. */ 103 localB->y[0] = rtu_u1; 104 localB->y[1] = rtu_u2; 105 localB->y[2] = rtu_u3; 106 } 107 108 /* Output and update for atomic system: 109 * '<S129>/Embedded MATLAB Function' 110 * '<S183>/Embedded MATLAB Function' 111 */ 112 void cont_EmbeddedMATLABFunction(const real32_T rtu_x[3], const real32_T rtu_y[3], 113 rtB_EmbeddedMATLABFunction_cont *localB) 114 { 115 /* Embedded MATLAB: '<S129>/Embedded MATLAB Function' */ 116 /* This block supports an embeddable subset of the MATLAB language. */ 117 /* See the help menu for details. */ 118 localB->xDoty = (rtu_x[0] * rtu_y[0] + rtu_x[1] * rtu_y[1]) + rtu_x[2] * rtu_y 119 [2]; 120 } 121 122 /* Output and update for atomic system: 123 * '<S149>/negprotect' 124 * '<S188>/negprotect' 125 * '<S214>/negprotect' 126 * '<S221>/negprotect' 127 * '<S247>/negprotect' 128 * '<S255>/negprotect' 129 * '<S264>/negprotect' 130 */ 131 void controlMCUSlug_negprotect_e(real32_T rtu_val, 132 rtB_negprotect_controlMCUSlug_p *localB) 133 { 134 /* Embedded MATLAB: '<S149>/negprotect' */ 135 if (rtu_val >= 1.000000047E-003F) { 136 /* This block supports an embeddable subset of the MATLAB language. */ 137 /* See the help menu for details. */ 138 localB->zpVal_k = rtu_val; 139 } else { 140 localB->zpVal_k = 1.000000047E-003F; 141 } 142 } 143 144 /* Output and update for atomic system: 145 * '<S150>/Embedded MATLAB Function' 146 * '<S189>/Embedded MATLAB Function' 147 */ 148 void co_EmbeddedMATLABFunction_a(const real32_T rtu_x[3], 149 rtB_EmbeddedMATLABFunction_co_e *localB) 150 { 151 /* Embedded MATLAB: '<S150>/Embedded MATLAB Function' */ 152 /* This block supports an embeddable subset of the MATLAB language. */ 153 /* See the help menu for details. */ 154 localB->xDoty_g = (rtu_x[0] * rtu_x[0] + rtu_x[1] * rtu_x[1]) + rtu_x[2] * 155 rtu_x[2]; 156 } 157 158 /* Output and update for atomic system: 159 * '<S215>/Embedded MATLAB Function' 160 * '<S222>/Embedded MATLAB Function' 161 * '<S248>/Embedded MATLAB Function' 162 */ 163 void co_EmbeddedMATLABFunction_h(const real32_T rtu_x[3], 164 rtB_EmbeddedMATLABFunction_co_l *localB) 165 { 166 /* Embedded MATLAB: '<S215>/Embedded MATLAB Function' */ 167 /* This block supports an embeddable subset of the MATLAB language. */ 168 /* See the help menu for details. */ 169 localB->xDoty_j = (rtu_x[0] * rtu_x[0] + rtu_x[1] * rtu_x[1]) + rtu_x[2] * 170 rtu_x[2]; 171 } 172 173 /* Output and update for atomic system: 174 * '<S293>/myMux Fun1' 175 * '<S296>/myMux Fun1' 176 * '<S294>/myMux Fun1' 177 * '<S295>/myMux Fun1' 178 */ 179 void controlMCUSlugs_myMuxFun1_a(uint16_T rtu_u1, uint16_T rtu_u2, uint16_T 180 rtu_u3, uint16_T rtu_u4, uint16_T rty_0[4]) 181 { 182 /* Embedded MATLAB: '<S293>/myMux Fun1' */ 183 /* This block supports an embeddable subset of the MATLAB language. */ 184 /* See the help menu for details. */ 185 rty_0[0] = rtu_u1; 186 rty_0[1] = rtu_u2; 187 rty_0[2] = rtu_u3; 188 rty_0[3] = rtu_u4; 189 } 190 191 /* Output and update for atomic system: 192 * '<S333>/myMux Fun1' 193 * '<S350>/myMux Fun1' 194 */ 195 void controlMCUSlugs_myMuxFun1_n(real32_T rtu_u1, real32_T rtu_u2, real32_T 196 rtu_u3, real32_T rtu_u4, real32_T rtu_u5, real32_T rtu_u6, real32_T rtu_u7, 197 real32_T rtu_u8, real32_T rtu_u9, rtB_myMuxFun1_controlMCUSlugs_p *localB) 198 { 199 /* Embedded MATLAB: '<S333>/myMux Fun1' */ 200 /* This block supports an embeddable subset of the MATLAB language. */ 201 /* See the help menu for details. */ 202 localB->y_l[0] = rtu_u1; 203 localB->y_l[1] = rtu_u2; 204 localB->y_l[2] = rtu_u3; 205 localB->y_l[3] = rtu_u4; 206 localB->y_l[4] = rtu_u5; 207 localB->y_l[5] = rtu_u6; 208 localB->y_l[6] = rtu_u7; 209 localB->y_l[7] = rtu_u8; 210 localB->y_l[8] = rtu_u9; 211 } 212 213 /* Output and update for atomic system: 214 * '<S334>/myMux Fun1' 215 * '<S334>/myMux Fun2' 216 * '<S351>/myMux Fun1' 217 * '<S351>/myMux Fun2' 218 * '<S9>/myMux Fun1' 219 */ 220 void controlMCUSlugs_myMuxFun1_o(real32_T rtu_u1, real32_T rtu_u2, real32_T 221 rtu_u3, rtB_myMuxFun1_controlMCUSlugs_b *localB) 222 { 223 /* Embedded MATLAB: '<S334>/myMux Fun1' */ 224 /* This block supports an embeddable subset of the MATLAB language. */ 225 /* See the help menu for details. */ 226 localB->y_i[0] = rtu_u1; 227 localB->y_i[1] = rtu_u2; 228 localB->y_i[2] = rtu_u3; 229 } 230 231 /* Model step function for TID0 */ 232 void controlMCUSlugsMKII_step0(void) /* Sample time: [0.01s, 0.0s] */ 233 { 234 /* local block i/o variables */ 235 real_T rtb_DataTypeConversion; 236 real_T rtb_IntegerDelay2; 237 real_T rtb_IC1; 238 real_T rtb_IntegerDelay2_b; 239 real_T rtb_Switch3; 240 real_T rtb_swIncrement; 241 real_T rtb_AntiWindup; 242 real_T rtb_Switch3_n; 243 real_T rtb_IntegerDelay; 244 real_T rtb_IntegerDelay1; 245 real_T rtb_AntiWindup_e; 246 real_T rtb_Switch3_k; 247 real_T rtb_IntegerDelay_g; 248 real_T rtb_IntegerDelay1_n; 249 real_T rtb_AntiWindup_d; 250 real_T rtb_Switch3_o; 251 real_T rtb_IntegerDelay_i; 252 real_T rtb_IntegerDelay1_j; 253 real_T rtb_AntiWindup_k; 254 real_T rtb_Switch3_g; 255 real_T rtb_IntegerDelay_b; 256 real_T rtb_IntegerDelay1_o; 257 real_T rtb_UnitConversion; 258 real_T rtb_AntiWindup_m; 259 real_T rtb_Switch3_na; 260 real_T rtb_IntegerDelay_c; 261 real_T rtb_IntegerDelay1_d; 262 real_T rtb_IntegerDelay1_c; 263 real_T rtb_IntegerDelay3; 264 real_T rtb_IntegerDelay3_l; 265 real_T rtb_IntegerDelay3_k; 266 real32_T rtb_Switch3_ov; 267 real32_T rtb_u020; 268 real32_T rtb_u5u6; 269 real32_T rtb_Sum; 270 real32_T rtb_Sum_o; 271 real32_T rtb_u3u5; 272 real32_T rtb_Sum_oz; 273 real32_T rtb_Sum_m; 274 real32_T rtb_Gain2; 275 real32_T rtb_u1u3_g; 276 real32_T rtb_u4u5; 277 real32_T rtb_u5u6_l; 278 real32_T rtb_Sum_od; 279 real32_T rtb_Sum_i; 280 real32_T rtb_u3u5_b; 281 real32_T rtb_Sum_p; 282 real32_T rtb_Sum_pu; 283 real32_T rtb_Gain2_m; 284 real32_T rtb_u1u3_l; 285 real32_T rtb_u4u5_a; 286 real32_T rtb_Product; 287 real32_T rtb_Subtract; 288 real32_T rtb_Switch3_oe; 289 real32_T rtb_Sum_l[3]; 290 real32_T rtb_y[4]; 291 real32_T rtb_y_d; 292 real32_T rtb_u525[3]; 293 real32_T rtb_Switch3_h; 294 real32_T rtb_u45; 295 real32_T rtb_Divide4; 296 real32_T rtb_Add_id[3]; 297 real32_T rtb_Switch3_nn; 298 real32_T rtb_Gain_m; 299 real32_T rtb_NPey[3]; 300 real32_T rtb_Divide5[3]; 301 real32_T rtb_Switch3_a; 302 real32_T rtb_Product_m; 303 real32_T rtb_Switch3_m; 304 real32_T rtb_Product_c; 305 real32_T rtb_stWP; 306 real32_T rtb_Saturation1; 307 real32_T rtb_Saturation1_a; 308 real32_T rtb_IC; 309 real32_T rtb_Subtract_h[3]; 310 real32_T rtb_DataTypeConversion_d; 311 real32_T rtb_DataTypeConversion2; 312 real32_T rtb_Switch3_e; 313 real32_T rtb_Switch3_c; 314 real32_T rtb_xDoty; 315 real32_T rtb_sinphi; 316 real32_T rtb_Rh; 317 real32_T rtb_Deg2R1; 318 real32_T rtb_Add_fn[3]; 319 real32_T rtb_sinphi_a; 320 real32_T rtb_Rh_o; 321 real32_T rtb_Deg2R1_j; 322 real32_T rtb_UEN2NEU[3]; 323 real32_T rtb_Add_d[3]; 324 real32_T rtb_Subtract1; 325 real32_T rtb_sinphi_m; 326 real32_T rtb_Rh_i; 327 real32_T rtb_Deg2R1_d; 328 real32_T rtb_Product1_m; 329 real32_T rtb_Add_k[3]; 330 real32_T rtb_Add_ir; 331 real32_T rtb_Product1_b; 332 real32_T rtb_sinphi_c; 333 real32_T rtb_Rh_m; 334 real32_T rtb_Deg2R1_g; 335 real32_T rtb_UEN2NEU_l[3]; 336 real32_T rtb_P; 337 real32_T rtb_Divide1_d; 338 real32_T rtb_Divide2; 339 real32_T rtb_Switch3_f; 340 real32_T rtb_ScheduleLPF; 341 real32_T rtb_Add2; 342 real32_T rtb_Switch3_c1; 343 real32_T rtb_IntegerDelay2_f; 344 real32_T rtb_Sum2; 345 real32_T rtb_Switch1; 346 real32_T rtb_Switch3_b; 347 real32_T rtb_Add2_i; 348 real32_T rtb_Theta_cLimit; 349 real32_T rtb_Switch3_mx; 350 real32_T rtb_Add_b1; 351 real32_T rtb_Add4; 352 real32_T rtb_Switch3_kr; 353 real32_T rtb_Product1_j; 354 real32_T rtb_NDelays; 355 real32_T rtb_IntegerDelay2_fm; 356 real32_T rtb_Sum2_k; 357 real32_T rtb_Add3; 358 real32_T rtb_Switch1_o; 359 real32_T rtb_Switch3_p; 360 real32_T rtb_Add2_d; 361 real32_T rtb_Add1; 362 real32_T rtb_Add3_i; 363 real32_T rtb_Add4_k; 364 real32_T rtb_Switch3_f3; 365 real32_T rtb_Product1_a; 366 real32_T rtb_NDelays_n; 367 real32_T rtb_IntegerDelay2_h; 368 real32_T rtb_Sum2_a; 369 real32_T rtb_Add3_b; 370 real32_T rtb_Switch1_k; 371 real32_T rtb_Switch3_el; 372 real32_T rtb_Add2_a; 373 real32_T rtb_Enforceupperlimit_m; 374 real32_T rtb_Add4_ki; 375 real32_T rtb_ElevatorLimit; 376 real32_T rtb_ThrottleLimit; 377 real32_T rtb_y_g; 378 real32_T rtb_y_o; 379 real32_T rtb_y_i; 380 real32_T rtb_Switch3_gn; 381 real32_T rtb_Switch3_h4; 382 real32_T rtb_PsiDotLimit; 383 real32_T rtb_Switch; 384 real32_T rtb_BankLimitCommand; 385 real32_T rtb_Switch3_pa; 386 real32_T rtb_u020_g; 387 real32_T rtb_Switch3_fx; 388 real32_T rtb_u2; 389 real32_T rtb_Switch2; 390 real32_T rtb_Switch3_my; 391 real32_T rtb_Add4_m; 392 real32_T rtb_Product1_p; 393 real32_T rtb_NDelays_no; 394 real32_T rtb_IntegerDelay2_ih; 395 real32_T rtb_Sum2_c; 396 real32_T rtb_Add3_k; 397 real32_T rtb_Switch1_i; 398 real32_T rtb_Switch3_pf; 399 real32_T rtb_Add2_af; 400 real32_T rtb_RudderLimit; 401 real32_T rtb_UnitConversion_a; 402 real32_T rtb_Add1_j; 403 real32_T rtb_Add4_p; 404 real32_T rtb_Switch3_i; 405 real32_T rtb_Product1_mf; 406 real32_T rtb_NDelays_m; 407 real32_T rtb_IntegerDelay2_k; 408 real32_T rtb_Sum2_b; 409 real32_T rtb_Add3_o; 410 real32_T rtb_Switch1_g; 411 real32_T rtb_Switch3_cv; 412 real32_T rtb_Add2_n; 413 real32_T rtb_AileronLimit; 414 real32_T rtb_y_om[3]; 415 real32_T rtb_Divide_l; 416 real32_T rtb_Divide1_a; 417 real32_T rtb_Divide_cm; 418 real32_T rtb_Switch3_pq; 419 real32_T rtb_IntegerDelay_k; 420 real32_T rtb_IntegerDelay1_a; 421 real32_T rtb_u2_o; 422 real32_T rtb_y_m; 423 real32_T rtb_IC_n; 424 real32_T rtb_Divide_p; 425 real32_T rtb_Subtract_b; 426 real32_T rtb_Switch3_cj; 427 real32_T rtb_IC_j; 428 real32_T rtb_Divide_f4; 429 real32_T rtb_Subtract_c; 430 real32_T rtb_Gain_ey; 431 real32_T rtb_Divide4_a; 432 real32_T rtb_Divide3; 433 real32_T rtb_Projection; 434 real32_T rtb_Switch3_ae; 435 real32_T rtb_IntegerDelay1_ov; 436 real32_T rtb_Switch3_gq; 437 real32_T rtb_Subtract1_d; 438 real32_T rtb_Switch3_im; 439 real32_T rtb_MathFunction[9]; 440 real32_T rtb_Product2_cp[3]; 441 real32_T rtb_Product3_ep[3]; 442 real32_T rtb_u525_m[3]; 443 real32_T rtb_Sum_mb[3]; 444 real32_T rtb_TmpHiddenBufferAtProduct1In[9]; 445 real32_T rtb_Product1_az[3]; 446 real32_T rtb_Deg2R_g; 447 real32_T rtb_Sum2_j; 448 real32_T rtb_Sum1_f[3]; 449 real32_T rtb_TmpHiddenBufferAtProduct1_h[9]; 450 real32_T rtb_Product1_jw[3]; 451 real32_T rtb_Deg2R_i; 452 real32_T rtb_Sum2_jc; 453 real32_T rtb_Add_n; 454 real32_T rtb_TmpHiddenBufferAtProduct1_g[9]; 455 real32_T rtb_Sum1_g[3]; 456 real32_T rtb_Product1_d4[3]; 457 real32_T rtb_O[3]; 458 real32_T rtb_Pch[3]; 459 real32_T rtb_PchB[3]; 460 real32_T rtb_Diff; 461 real32_T rtb_Subtract_a; 462 real32_T rtb_IntegerDelay2_j; 463 real32_T rtb_Sum2_d; 464 real32_T rtb_Deg2R_c; 465 real32_T rtb_UEN2NEU_f[3]; 466 real32_T rtb_Sum2_g0; 467 real32_T rtb_Deg2R_a; 468 real32_T rtb_Enforcelowerlimit_c; 469 real32_T rtb_Product1_ma; 470 real32_T rtb_Saturation1_pt; 471 real32_T rtb_jxk; 472 real32_T rtb_Add1_b; 473 real32_T rtb_psi_ad; 474 real32_T rtb_Gain2_f; 475 real32_T rtb_IntegerDelay3_j; 476 real32_T rtb_Abs2; 477 real32_T rtb_Sum_k; 478 real32_T rtb_Add1_l; 479 real32_T rtb_IntegerDelay3_d; 480 real32_T rtb_IntegerDelay3_hu; 481 real32_T rtb_IntegerDelay3_p; 482 uint16_T rtb_Switch_k; 483 uint16_T rtb_Switch1_bs; 484 uint16_T rtb_Switch2_l; 485 uint16_T rtb_Switch3_c5; 486 uint16_T rtb_Gain_pn; 487 uint16_T rtb_Gain1_c; 488 uint16_T rtb_Gain2_d; 489 uint16_T rtb_Gain3_p; 490 uint16_T rtb_DataTypeConversion_k; 491 uint16_T rtb_DataTypeConversion_o; 492 uint16_T rtb_DataTypeConversion_o5; 493 uint16_T rtb_DataTypeConversion_j; 494 uint16_T rtb_Gain_fi; 495 uint16_T rtb_Gain1_cp; 496 uint16_T rtb_Gain2_fp; 497 uint16_T rtb_Gain3_j; 498 uint16_T rtb_y_n[5]; 499 uint16_T rtb_TmpHiddenBufferAtSFunctionI[4]; 500 uint8_T rtb_Add_mo; 501 uint8_T rtb_RelationalOperator2; 502 uint8_T rtb_RelationalOperator2_k; 503 uint8_T rtb_Add1_i; 504 uint8_T rtb_RelationalOperator2_g; 505 uint8_T rtb_IntegerDelay1_l; 506 uint8_T rtb_RelationalOperator2_i; 507 uint8_T rtb_SwitchDistanceLessthan; 508 uint8_T rtb_RelationalOperator2_h; 509 uint8_T rtb_Compare_l; 510 uint8_T rtb_RelationalOperator2_a; 511 uint8_T rtb_RelationalOperator2_j4; 512 uint8_T rtb_RelationalOperator2_m3; 513 uint8_T rtb_LogicalOperator; 514 uint8_T rtb_LogicalOperator2; 515 boolean_T rtb_LogicalOperator_e; 516 boolean_T rtb_Largerthan180; 517 boolean_T rtb_IntegerDelay1_oe; 518 519 { /* Sample time: [0.01s, 0.0s] */ 520 rate_monotonic_scheduler(); 521 } 522 523 /* S-Function "dsPIC_PWM_IC" Block: <S277>/Input Capture */ 524 controlMCUSlugsMKII_B.InputCapture_o1 = ic2up; 525 controlMCUSlugsMKII_B.InputCapture_o2 = ic3up; 526 controlMCUSlugsMKII_B.InputCapture_o3 = ic4up; 527 controlMCUSlugsMKII_B.InputCapture_o4 = ic5up; 528 controlMCUSlugsMKII_B.InputCapture_o5 = ic8up; 529 530 /* Embedded MATLAB: '<S277>/myMux Fun5' */ 531 532 /* This block supports an embeddable subset of the MATLAB language. */ 533 /* See the help menu for details. */ 534 rtb_y_n[0] = controlMCUSlugsMKII_B.InputCapture_o1; 535 rtb_y_n[1] = controlMCUSlugsMKII_B.InputCapture_o2; 536 rtb_y_n[2] = controlMCUSlugsMKII_B.InputCapture_o3; 537 rtb_y_n[3] = controlMCUSlugsMKII_B.InputCapture_o4; 538 rtb_y_n[4] = controlMCUSlugsMKII_B.InputCapture_o5; 539 540 /* S-Function "dsPIC_C_function_Call" Block: <S8>/Get Control Type [navSupport.c] */ 541 controlMCUSlugsMKII_B.GetControlTypenavSupportc = getApControlType(); 542 543 /* S-Function "UART_RX" Block: <S11>/Calculus Time Step */ 544 controlMCUSlugsMKII_B.CalculusTimeStep_o1 = CalculusTimeStep; 545 controlMCUSlugsMKII_B.CalculusTimeStep_o2 = PR1; 546 547 { 548 real_T tmp; 549 550 /* DataTypeConversion: '<S11>/Data Type Conversion12' incorporates: 551 * DataTypeConversion: '<S11>/Data Type Conversion3' 552 * DataTypeConversion: '<S11>/Data Type Conversion4' 553 * Gain: '<S11>/Gain' 554 * Product: '<S11>/Divide' 555 * Rounding: '<S11>/Rounding Function' 556 */ 557 tmp = fmod(floor(floor((real_T)controlMCUSlugsMKII_B.CalculusTimeStep_o1 / 558 (real_T)controlMCUSlugsMKII_B.CalculusTimeStep_o2 * 100.0)), 256.0); 559 if (tmp < 0.0) { 560 tmp += 256.0; 561 } 562 563 controlMCUSlugsMKII_B.DataTypeConversion12 = (uint8_T)tmp; 564 } 565 566 /* S-Function "dsPIC_C_function_Call" Block: <S1>/Data from GS [groundStationDriver.c] */ 567 gsRead(&controlMCUSlugsMKII_B.DatafromGSgroundStationDriverc[0]); 568 569 /* S-Function "dsPIC_C_function_Call" Block: <S1>/Data from IPC [interProcCommSlave.c] */ 570 readIpc(&controlMCUSlugsMKII_B.DatafromIPCinterProcCommSlavec[0]); 571 572 /* S-Function "dsPIC_C_function_Call" Block: <S7>/Parser//Decoder [protDecoder.c] */ 573 protParseDecode(controlMCUSlugsMKII_B.DatafromIPCinterProcCommSlavec); 574 575 /* S-Function "dsPIC_C_function_Call" Block: <S7>/GS Messages Parser//Decoder [groundStationDriver.c] */ 576 protParseDecodeGS(controlMCUSlugsMKII_B.DatafromGSgroundStationDriverc); 577 578 /* S-Function "dsPIC_C_function_Call" Block: <S7>/update Load of Control MCU [groundStationDriver.c] */ 579 updateLoad(controlMCUSlugsMKII_B.DataTypeConversion12); 580 581 /* S-Function "dsPIC_C_function_Call" Block: <Root>/Get MidLevel Commands [navSupport.c] */ 582 getMidLevelCommands(&controlMCUSlugsMKII_B.GetMidLevelCommandsnavSupportc[0]); 583 584 /* S-Function "dsPIC_C_function_Call" Block: <Root>/Get Dynamic P [navSupport.c] */ 585 controlMCUSlugsMKII_B.GetDynamicPnavSupportc = getDynamic(); 586 587 /* S-Function "dsPIC_C_function_Call" Block: <S3>/Get Pid according to Index */ 588 getPidIdx(((uint8_T)0U),&controlMCUSlugsMKII_B.GetPidaccordingtoIndex[0]); 589 590 /* Embedded MATLAB: '<S276>/Buffer IC Channel' */ 591 { 592 int16_T eml_i; 593 594 /* This block supports an embeddable subset of the MATLAB language. */ 595 /* See the help menu for details. */ 596 for (eml_i = 0; eml_i < 7; eml_i++) { 597 controlMCUSlugsMKII_B.history_n[eml_i] = 0U; 598 } 599 600 for (eml_i = 7; eml_i > 1; eml_i += -1) { 601 controlMCUSlugsMKII_DWork.BufferICChannel.oldValues[eml_i - 1] = 602 controlMCUSlugsMKII_DWork.BufferICChannel.oldValues[eml_i - 2]; 603 controlMCUSlugsMKII_B.history_n[eml_i - 1] = 604 controlMCUSlugsMKII_DWork.BufferICChannel.oldValues[eml_i - 2]; 605 } 606 607 controlMCUSlugsMKII_DWork.BufferICChannel.oldValues[0] = rtb_y_n[0]; 608 controlMCUSlugsMKII_B.history_n[0] = rtb_y_n[0]; 609 } 610 611 /* Embedded MATLAB: '<S276>/Buffer IC Channel1' */ 612 { 613 int16_T eml_i; 614 615 /* This block supports an embeddable subset of the MATLAB language. */ 616 /* See the help menu for details. */ 617 for (eml_i = 0; eml_i < 7; eml_i++) { 618 controlMCUSlugsMKII_B.history_ec[eml_i] = 0U; 619 } 620 621 for (eml_i = 7; eml_i > 1; eml_i += -1) { 622 controlMCUSlugsMKII_DWork.BufferICChannel1.oldValues[eml_i - 1] = 623 controlMCUSlugsMKII_DWork.BufferICChannel1.oldValues[eml_i - 2]; 624 controlMCUSlugsMKII_B.history_ec[eml_i - 1] = 625 controlMCUSlugsMKII_DWork.BufferICChannel1.oldValues[eml_i - 2]; 626 } 627 628 controlMCUSlugsMKII_DWork.BufferICChannel1.oldValues[0] = rtb_y_n[1]; 629 controlMCUSlugsMKII_B.history_ec[0] = rtb_y_n[1]; 630 } 631 632 /* Embedded MATLAB: '<S276>/Buffer IC Channel2' */ 633 { 634 int16_T eml_i; 635 636 /* This block supports an embeddable subset of the MATLAB language. */ 637 /* See the help menu for details. */ 638 for (eml_i = 0; eml_i < 7; eml_i++) { 639 controlMCUSlugsMKII_B.history_e[eml_i] = 0U; 640 } 641 642 for (eml_i = 7; eml_i > 1; eml_i += -1) { 643 controlMCUSlugsMKII_DWork.BufferICChannel2.oldValues[eml_i - 1] = 644 controlMCUSlugsMKII_DWork.BufferICChannel2.oldValues[eml_i - 2]; 645 controlMCUSlugsMKII_B.history_e[eml_i - 1] = 646 controlMCUSlugsMKII_DWork.BufferICChannel2.oldValues[eml_i - 2]; 647 } 648 649 controlMCUSlugsMKII_DWork.BufferICChannel2.oldValues[0] = rtb_y_n[3]; 650 controlMCUSlugsMKII_B.history_e[0] = rtb_y_n[3]; 651 } 652 653 /* Embedded MATLAB: '<S276>/Buffer IC Channel3' */ 654 { 655 int16_T eml_i; 656 657 /* This block supports an embeddable subset of the MATLAB language. */ 658 /* See the help menu for details. */ 659 for (eml_i = 0; eml_i < 7; eml_i++) { 660 controlMCUSlugsMKII_B.history[eml_i] = 0U; 661 } 662 663 for (eml_i = 7; eml_i > 1; eml_i += -1) { 664 controlMCUSlugsMKII_DWork.BufferICChannel3.oldValues[eml_i - 1] = 665 controlMCUSlugsMKII_DWork.BufferICChannel3.oldValues[eml_i - 2]; 666 controlMCUSlugsMKII_B.history[eml_i - 1] = 667 controlMCUSlugsMKII_DWork.BufferICChannel3.oldValues[eml_i - 2]; 668 } 669 670 controlMCUSlugsMKII_DWork.BufferICChannel3.oldValues[0] = rtb_y_n[2]; 671 controlMCUSlugsMKII_B.history[0] = rtb_y_n[2]; 672 } 673 674 /* S-Function (sfix_udelay): '<S6>/Integer Delay' 675 * 676 * Regarding '<S6>/Integer Delay': 677 * Integer/Tapped Delay Block: '<S6>/Integer Delay' 678 */ 679 controlMCUSlugsMKII_B.IntegerDelay = 680 controlMCUSlugsMKII_DWork.IntegerDelay_X_p; 681 682 /* S-Function "dsPIC_C_function_Call" Block: <S6>/Manual or Auto? [navSupport.c] */ 683 controlMCUSlugsMKII_B.ManualorAutonavSupportc = isApManual(); 684 685 { 686 ZCEventType zcEvent; 687 688 /* DataTypeConversion: '<S278>/Data Type Conversion' */ 689 rtb_DataTypeConversion = (real_T) 690 controlMCUSlugsMKII_B.ManualorAutonavSupportc; 691 692 /* HitCross: '<S278>/Hit Crossing' */ 693 zcEvent = rt_ZCFcn(FALLING_ZERO_CROSSING, 694 &controlMCUSlugsM_PrevZCSigState.HitCrossing_ZCE, 695 (rtb_DataTypeConversion - 0.5)); 696 if (controlMCUSlugsMKII_DWork.HitCrossing_MODE == 0L) { 697 if (zcEvent != NO_ZCEVENT) { 698 controlMCUSlugsMKII_B.HitCrossing = ((controlMCUSlugsMKII_B.HitCrossing == 699 1 ? 0 : 1) != 0); 700 controlMCUSlugsMKII_DWork.HitCrossing_MODE = 1L; 701 } else if ((controlMCUSlugsMKII_B.HitCrossing == 1) && 702 (rtb_DataTypeConversion != 0.5)) { 703 controlMCUSlugsMKII_B.HitCrossing = 0U; 704 } 705 } else { 706 if (rtb_DataTypeConversion != 0.5) { 707 controlMCUSlugsMKII_B.HitCrossing = 0U; 708 } 709 710 controlMCUSlugsMKII_DWork.HitCrossing_MODE = 0L; 711 } 712 713 /* InitialCondition: '<S278>/IC' */ 714 if (controlMCUSlugsMKII_DWork.IC_FirstOutputTime) { 715 controlMCUSlugsMKII_DWork.IC_FirstOutputTime = 0U; 716 controlMCUSlugsMKII_B.IC_c = 1U; 717 } else { 718 controlMCUSlugsMKII_B.IC_c = controlMCUSlugsMKII_B.HitCrossing; 719 } 720 721 /* Sum: '<S278>/Add' incorporates: 722 * S-Function (sfix_udelay): '<S278>/Integer Delay2' 723 */ 724 rtb_Add_mo = (uint8_T)((uint16_T) 725 (controlMCUSlugsMKII_DWork.IntegerDelay2_X_k) + (uint16_T) 726 controlMCUSlugsMKII_B.IC_c); 727 if (rtb_Add_mo > 0U) { 728 if (controlMCUSlugsMKII_DWork.GrabIC_MODE == (int32_T)SUBSYS_DISABLED) { 729 controlMCUSlugsMKII_DWork.GrabIC_MODE = (int32_T)SUBSYS_ENABLED; 730 } 731 } else if (controlMCUSlugsMKII_DWork.GrabIC_MODE == (int32_T)SUBSYS_ENABLED) 732 { 733 controlMCUSlugsMKII_DWork.GrabIC_MODE = (int32_T)SUBSYS_DISABLED; 734 } 735 736 if (controlMCUSlugsMKII_DWork.GrabIC_MODE == (int32_T)SUBSYS_ENABLED) { 737 /* Output and update for enable system: '<S6>/Grab I.C.' */ 738 739 /* S-Function "dsPIC_C_function_Call" Block: <S279>/Choose the Median [navSupport.c] */ 740 controlMCUSlugsMKII_B.ChoosetheMediannavSupportc = meanFilter5 741 (controlMCUSlugsMKII_B.history_n); 742 743 /* DataTypeConversion: '<S286>/Data Type Conversion' incorporates: 744 * Constant: '<S286>/Constant1' 745 * Constant: '<S286>/Constant2' 746 * Product: '<S286>/Divide' 747 * Sum: '<S286>/Add' 748 */ 749 controlMCUSlugsMKII_B.DataTypeConversion = (real32_T)((real_T) 750 controlMCUSlugsMKII_B.ChoosetheMediannavSupportc * 751 3.9246467817896402E-004 + -2.4929356357927808E+000); 752 753 /* S-Function "dsPIC_C_function_Call" Block: <S279>/Choose the Median [navSupport.c]1 */ 754 controlMCUSlugsMKII_B.ChoosetheMediannavSupportc1 = meanFilter5 755 (controlMCUSlugsMKII_B.history_ec); 756 757 /* DataTypeConversion: '<S287>/Data Type Conversion' incorporates: 758 * Constant: '<S287>/Constant1' 759 * Constant: '<S287>/Constant2' 760 * Product: '<S287>/Divide' 761 * Sum: '<S287>/Add' 762 */ 763 controlMCUSlugsMKII_B.DataTypeConversion_f = (real32_T)((real_T) 764 controlMCUSlugsMKII_B.ChoosetheMediannavSupportc1 * 765 1.4565320234764749E-004 + -1.0755448613357002E+000); 766 767 /* S-Function "dsPIC_C_function_Call" Block: <S279>/Choose the Median [navSupport.c]2 */ 768 controlMCUSlugsMKII_B.ChoosetheMediannavSupportc2 = meanFilter5 769 (controlMCUSlugsMKII_B.history_e); 770 771 /* DataTypeConversion: '<S288>/Data Type Conversion' incorporates: 772 * Constant: '<S288>/Constant1' 773 * Constant: '<S288>/Constant2' 774 * Product: '<S288>/Divide' 775 * Sum: '<S288>/Add' 776 */ 777 controlMCUSlugsMKII_B.DataTypeConversion_b = (real32_T)((real_T) 778 controlMCUSlugsMKII_B.ChoosetheMediannavSupportc2 * 779 1.7474741230625283E-004 + -1.3006200258793963E+000); 780 781 /* S-Function "dsPIC_C_function_Call" Block: <S279>/Choose the Median [navSupport.c]3 */ 782 controlMCUSlugsMKII_B.ChoosetheMediannavSupportc3 = meanFilter5 783 (controlMCUSlugsMKII_B.history); 784 785 /* DataTypeConversion: '<S289>/Data Type Conversion' incorporates: 786 * Constant: '<S289>/Constant1' 787 * Constant: '<S289>/Constant2' 788 * Product: '<S289>/Divide' 789 * Sum: '<S289>/Add' 790 */ 791 controlMCUSlugsMKII_B.DataTypeConversion_c = (real32_T)((real_T) 792 controlMCUSlugsMKII_B.ChoosetheMediannavSupportc3 * 793 1.2648319566552172E-004 + -9.5530950783373303E-001); 794 795 /* S-Function "dsPIC_C_function_Call" Block: <S279>/Set Current Height and Airspeed as Commands [navSupport.c] */ 796 setCurrentCommands(controlMCUSlugsMKII_B.IntegerDelay); 797 } 798 } 799 800 /* S-Function "dsPIC_C_function_Call" Block: <Root>/Get XYZ [navSupport.c] */ 801 getXYZ(&controlMCUSlugsMKII_B.GetXYZnavSupportc[0]); 802 803 /* RelationalOperator: '<S17>/Relational Operator2' */ 804 rtb_RelationalOperator2 = (controlMCUSlugsMKII_B.GetXYZnavSupportc[2] == 805 controlMCUSlugsMKII_B.GetXYZnavSupportc[2]); 806 807 /* Switch: '<S17>/Switch3' incorporates: 808 * S-Function (sfix_udelay): '<S17>/Integer Delay3' 809 */ 810 if (rtb_RelationalOperator2 > 0U) { 811 rtb_Switch3_ov = controlMCUSlugsMKII_B.GetXYZnavSupportc[2]; 812 } else { 813 rtb_Switch3_ov = (controlMCUSlugsMKII_DWork.IntegerDelay3_X_j5); 814 } 815 816 /* Saturate: '<S2>/[-20 20]' */ 817 rtb_u020 = rt_SATURATE(rtb_Switch3_ov, 170.0F, 350.0F); 818 819 /* S-Function "dsPIC_C_function_Call" Block: <S3>/Get Pid according to Index1 */ 820 getPidIdx(((uint8_T)1U),&controlMCUSlugsMKII_B.GetPidaccordingtoIndex1[0]); 821 822 /* S-Function "dsPIC_C_function_Call" Block: <S9>/[navSupport.c] */ 823 getPidIdx(((uint8_T)7U),&controlMCUSlugsMKII_B.navSupportc[0]); 824 825 /* Gain: '<S309>/Unit Conversion' */ 826 controlMCUSlugsMKII_B.UnitConversion[0] = 1.745329238E-002F * 827 controlMCUSlugsMKII_B.navSupportc[0]; 828 controlMCUSlugsMKII_B.UnitConversion[1] = 1.745329238E-002F * 829 controlMCUSlugsMKII_B.navSupportc[1]; 830 controlMCUSlugsMKII_B.UnitConversion[2] = 1.745329238E-002F * 831 controlMCUSlugsMKII_B.navSupportc[2]; 832 833 /* S-Function "dsPIC_C_function_Call" Block: <S334>/C Function Call1 */ 834 controlMCUSlugsMKII_B.CFunctionCall1 = myCos 835 (controlMCUSlugsMKII_B.UnitConversion[0]); 836 837 /* S-Function "dsPIC_C_function_Call" Block: <S334>/C Function Call4 */ 838 controlMCUSlugsMKII_B.CFunctionCall4 = myCos 839 (controlMCUSlugsMKII_B.UnitConversion[1]); 840 841 /* S-Function "dsPIC_C_function_Call" Block: <S334>/C Function Call5 */ 842 controlMCUSlugsMKII_B.CFunctionCall5 = myCos 843 (controlMCUSlugsMKII_B.UnitConversion[2]); 844 845 /* Stateflow: '<S334>/myMux Fun2' */ 846 controlMCUSlugs_myMuxFun1_o(controlMCUSlugsMKII_B.CFunctionCall1, 847 controlMCUSlugsMKII_B.CFunctionCall4, controlMCUSlugsMKII_B.CFunctionCall5, 848 &controlMCUSlugsMKII_B.sf_myMuxFun2); 849 850 /* Product: '<S324>/u(5)*u(6)' */ 851 rtb_u5u6 = controlMCUSlugsMKII_B.sf_myMuxFun2.y_i[1] * 852 controlMCUSlugsMKII_B.sf_myMuxFun2.y_i[2]; 853 854 /* S-Function "dsPIC_C_function_Call" Block: <S334>/C Function Call */ 855 controlMCUSlugsMKII_B.CFunctionCall = mySin 856 (controlMCUSlugsMKII_B.UnitConversion[0]); 857 858 /* S-Function "dsPIC_C_function_Call" Block: <S334>/C Function Call2 */ 859 controlMCUSlugsMKII_B.CFunctionCall2 = mySin 860 (controlMCUSlugsMKII_B.UnitConversion[1]); 861 862 /* S-Function "dsPIC_C_function_Call" Block: <S334>/C Function Call3 */ 863 controlMCUSlugsMKII_B.CFunctionCall3 = mySin 864 (controlMCUSlugsMKII_B.UnitConversion[2]); 865 866 /* Stateflow: '<S334>/myMux Fun1' */ 867 controlMCUSlugs_myMuxFun1_o(controlMCUSlugsMKII_B.CFunctionCall, 868 controlMCUSlugsMKII_B.CFunctionCall2, controlMCUSlugsMKII_B.CFunctionCall3, 869 &controlMCUSlugsMKII_B.sf_myMuxFun1); 870 871 /* Sum: '<S327>/Sum' incorporates: 872 * Product: '<S327>/u(3)*u(4)' 873 * Product: '<S327>/u(6)*u(1)*u(2)' 874 */ 875 rtb_Sum = controlMCUSlugsMKII_B.sf_myMuxFun2.y_i[2] * 876 controlMCUSlugsMKII_B.sf_myMuxFun1.y_i[0] * 877 controlMCUSlugsMKII_B.sf_myMuxFun1.y_i[1] - 878 controlMCUSlugsMKII_B.sf_myMuxFun1.y_i[2] * 879 controlMCUSlugsMKII_B.sf_myMuxFun2.y_i[0]; 880 881 /* Sum: '<S330>/Sum' incorporates: 882 * Product: '<S330>/u(1)*u(3)' 883 * Product: '<S330>/u(2)*u(4)*u(6)' 884 */ 885 rtb_Sum_o = controlMCUSlugsMKII_B.sf_myMuxFun1.y_i[1] * 886 controlMCUSlugsMKII_B.sf_myMuxFun2.y_i[0] * 887 controlMCUSlugsMKII_B.sf_myMuxFun2.y_i[2] + 888 controlMCUSlugsMKII_B.sf_myMuxFun1.y_i[0] * 889 controlMCUSlugsMKII_B.sf_myMuxFun1.y_i[2]; 890 891 /* Product: '<S325>/u(3)*u(5)' */ 892 rtb_u3u5 = controlMCUSlugsMKII_B.sf_myMuxFun1.y_i[2] * 893 controlMCUSlugsMKII_B.sf_myMuxFun2.y_i[1]; 894 895 /* Sum: '<S328>/Sum' incorporates: 896 * Product: '<S328>/u(1)*u(2)*u(3)' 897 * Product: '<S328>/u(4)*u(6)' 898 */ 899 rtb_Sum_oz = controlMCUSlugsMKII_B.sf_myMuxFun1.y_i[0] * 900 controlMCUSlugsMKII_B.sf_myMuxFun1.y_i[1] * 901 controlMCUSlugsMKII_B.sf_myMuxFun1.y_i[2] + 902 controlMCUSlugsMKII_B.sf_myMuxFun2.y_i[0] * 903 controlMCUSlugsMKII_B.sf_myMuxFun2.y_i[2]; 904 905 /* Sum: '<S331>/Sum' incorporates: 906 * Product: '<S331>/u(1)*u(6)' 907 * Product: '<S331>/u(2)*u(3)*u(4)' 908 */ 909 rtb_Sum_m = controlMCUSlugsMKII_B.sf_myMuxFun1.y_i[1] * 910 controlMCUSlugsMKII_B.sf_myMuxFun1.y_i[2] * 911 controlMCUSlugsMKII_B.sf_myMuxFun2.y_i[0] - 912 controlMCUSlugsMKII_B.sf_myMuxFun2.y_i[2] * 913 controlMCUSlugsMKII_B.sf_myMuxFun1.y_i[0]; 914 915 /* Gain: '<S326>/Gain2' */ 916 rtb_Gain2 = -1.0F * controlMCUSlugsMKII_B.sf_myMuxFun1.y_i[1]; 917 918 /* Product: '<S329>/u(1)*u(3)' */ 919 rtb_u1u3_g = controlMCUSlugsMKII_B.sf_myMuxFun1.y_i[0] * 920 controlMCUSlugsMKII_B.sf_myMuxFun2.y_i[1]; 921 922 /* Product: '<S332>/u(4)*u(5)' */ 923 rtb_u4u5 = controlMCUSlugsMKII_B.sf_myMuxFun2.y_i[0] * 924 controlMCUSlugsMKII_B.sf_myMuxFun2.y_i[1]; 925 926 /* Stateflow: '<S333>/myMux Fun1' */ 927 controlMCUSlugs_myMuxFun1_n(rtb_u5u6, rtb_Sum, rtb_Sum_o, rtb_u3u5, rtb_Sum_oz, 928 rtb_Sum_m, rtb_Gain2, rtb_u1u3_g, rtb_u4u5, 929 &controlMCUSlugsMKII_B.sf_myMuxFun1_n); 930 931 /* Math: '<S9>/Math Function' */ 932 933 /* Operator : transpose */ 934 { 935 uint32_T count = 0; 936 uint32_T row; 937 uint32_T col; 938 for (row= 0; row < 3; row++) { 939 for (col= 0; col < 3; col++) { 940 rtb_MathFunction[row + 3 * col] = 941 controlMCUSlugsMKII_B.sf_myMuxFun1_n.y_l[count]; 942 count++; 943 } 944 } 945 } 946 947 /* S-Function "dsPIC_C_function_Call" Block: <S9>/Get Attitude [navSupport.c] */ 948 getAttitude(&controlMCUSlugsMKII_B.GetAttitudenavSupportc[0]); 949 950 /* S-Function "dsPIC_C_function_Call" Block: <S351>/C Function Call1 */ 951 controlMCUSlugsMKII_B.CFunctionCall1_g = myCos 952 (controlMCUSlugsMKII_B.GetAttitudenavSupportc[0]); 953 954 /* S-Function "dsPIC_C_function_Call" Block: <S351>/C Function Call4 */ 955 controlMCUSlugsMKII_B.CFunctionCall4_a = myCos 956 (controlMCUSlugsMKII_B.GetAttitudenavSupportc[1]); 957 958 /* S-Function "dsPIC_C_function_Call" Block: <S351>/C Function Call5 */ 959 controlMCUSlugsMKII_B.CFunctionCall5_d = myCos 960 (controlMCUSlugsMKII_B.GetAttitudenavSupportc[2]); 961 962 /* Stateflow: '<S351>/myMux Fun2' */ 963 controlMCUSlugs_myMuxFun1_o(controlMCUSlugsMKII_B.CFunctionCall1_g, 964 controlMCUSlugsMKII_B.CFunctionCall4_a, 965 controlMCUSlugsMKII_B.CFunctionCall5_d, 966 &controlMCUSlugsMKII_B.sf_myMuxFun2_c); 967 968 /* Product: '<S341>/u(5)*u(6)' */ 969 rtb_u5u6_l = controlMCUSlugsMKII_B.sf_myMuxFun2_c.y_i[1] * 970 controlMCUSlugsMKII_B.sf_myMuxFun2_c.y_i[2]; 971 972 /* S-Function "dsPIC_C_function_Call" Block: <S351>/C Function Call */ 973 controlMCUSlugsMKII_B.CFunctionCall_i = mySin 974 (controlMCUSlugsMKII_B.GetAttitudenavSupportc[0]); 975 976 /* S-Function "dsPIC_C_function_Call" Block: <S351>/C Function Call2 */ 977 controlMCUSlugsMKII_B.CFunctionCall2_e = mySin 978 (controlMCUSlugsMKII_B.GetAttitudenavSupportc[1]); 979 980 /* S-Function "dsPIC_C_function_Call" Block: <S351>/C Function Call3 */ 981 controlMCUSlugsMKII_B.CFunctionCall3_i = mySin 982 (controlMCUSlugsMKII_B.GetAttitudenavSupportc[2]); 983 984 /* Stateflow: '<S351>/myMux Fun1' */ 985 controlMCUSlugs_myMuxFun1_o(controlMCUSlugsMKII_B.CFunctionCall_i, 986 controlMCUSlugsMKII_B.CFunctionCall2_e, 987 controlMCUSlugsMKII_B.CFunctionCall3_i, 988 &controlMCUSlugsMKII_B.sf_myMuxFun1_a); 989 990 /* Sum: '<S344>/Sum' incorporates: 991 * Product: '<S344>/u(3)*u(4)' 992 * Product: '<S344>/u(6)*u(1)*u(2)' 993 */ 994 rtb_Sum_od = controlMCUSlugsMKII_B.sf_myMuxFun2_c.y_i[2] * 995 controlMCUSlugsMKII_B.sf_myMuxFun1_a.y_i[0] * 996 controlMCUSlugsMKII_B.sf_myMuxFun1_a.y_i[1] - 997 controlMCUSlugsMKII_B.sf_myMuxFun1_a.y_i[2] * 998 controlMCUSlugsMKII_B.sf_myMuxFun2_c.y_i[0]; 999 1000 /* Sum: '<S347>/Sum' incorporates: 1001 * Product: '<S347>/u(1)*u(3)' 1002 * Product: '<S347>/u(2)*u(4)*u(6)' 1003 */ 1004 rtb_Sum_i = controlMCUSlugsMKII_B.sf_myMuxFun1_a.y_i[1] * 1005 controlMCUSlugsMKII_B.sf_myMuxFun2_c.y_i[0] * 1006 controlMCUSlugsMKII_B.sf_myMuxFun2_c.y_i[2] + 1007 controlMCUSlugsMKII_B.sf_myMuxFun1_a.y_i[0] * 1008 controlMCUSlugsMKII_B.sf_myMuxFun1_a.y_i[2]; 1009 1010 /* Product: '<S342>/u(3)*u(5)' */ 1011 rtb_u3u5_b = controlMCUSlugsMKII_B.sf_myMuxFun1_a.y_i[2] * 1012 controlMCUSlugsMKII_B.sf_myMuxFun2_c.y_i[1]; 1013 1014 /* Sum: '<S345>/Sum' incorporates: 1015 * Product: '<S345>/u(1)*u(2)*u(3)' 1016 * Product: '<S345>/u(4)*u(6)' 1017 */ 1018 rtb_Sum_p = controlMCUSlugsMKII_B.sf_myMuxFun1_a.y_i[0] * 1019 controlMCUSlugsMKII_B.sf_myMuxFun1_a.y_i[1] * 1020 controlMCUSlugsMKII_B.sf_myMuxFun1_a.y_i[2] + 1021 controlMCUSlugsMKII_B.sf_myMuxFun2_c.y_i[0] * 1022 controlMCUSlugsMKII_B.sf_myMuxFun2_c.y_i[2]; 1023 1024 /* Sum: '<S348>/Sum' incorporates: 1025 * Product: '<S348>/u(1)*u(6)' 1026 * Product: '<S348>/u(2)*u(3)*u(4)' 1027 */ 1028 rtb_Sum_pu = controlMCUSlugsMKII_B.sf_myMuxFun1_a.y_i[1] * 1029 controlMCUSlugsMKII_B.sf_myMuxFun1_a.y_i[2] * 1030 controlMCUSlugsMKII_B.sf_myMuxFun2_c.y_i[0] - 1031 controlMCUSlugsMKII_B.sf_myMuxFun2_c.y_i[2] * 1032 controlMCUSlugsMKII_B.sf_myMuxFun1_a.y_i[0]; 1033 1034 /* Gain: '<S343>/Gain2' */ 1035 rtb_Gain2_m = -1.0F * controlMCUSlugsMKII_B.sf_myMuxFun1_a.y_i[1]; 1036 1037 /* Product: '<S346>/u(1)*u(3)' */ 1038 rtb_u1u3_l = controlMCUSlugsMKII_B.sf_myMuxFun1_a.y_i[0] * 1039 controlMCUSlugsMKII_B.sf_myMuxFun2_c.y_i[1]; 1040 1041 /* Product: '<S349>/u(4)*u(5)' */ 1042 rtb_u4u5_a = controlMCUSlugsMKII_B.sf_myMuxFun2_c.y_i[0] * 1043 controlMCUSlugsMKII_B.sf_myMuxFun2_c.y_i[1]; 1044 1045 /* Stateflow: '<S350>/myMux Fun1' */ 1046 controlMCUSlugs_myMuxFun1_n(rtb_u5u6_l, rtb_Sum_od, rtb_Sum_i, rtb_u3u5_b, 1047 rtb_Sum_p, rtb_Sum_pu, rtb_Gain2_m, rtb_u1u3_l, rtb_u4u5_a, 1048 &controlMCUSlugsMKII_B.sf_myMuxFun1_m); 1049 1050 { 1051 static const int_T dims[3] = { 3, 3, 3 }; 1052 1053 rt_MatMultRR_Sgl(controlMCUSlugsMKII_B.Product, rtb_MathFunction, 1054 controlMCUSlugsMKII_B.sf_myMuxFun1_m.y_l, &dims[0]); 1055 } 1056 1057 /* S-Function "dsPIC_C_function_Call" Block: <S317>/C Function Call */ 1058 controlMCUSlugsMKII_B.CFunctionCall_i0 = myAtan2(controlMCUSlugsMKII_B.Product 1059 [7],controlMCUSlugsMKII_B.Product[8]); 1060 1061 /* Gain: '<S323>/Gain2' */ 1062 rtb_Abs2 = -1.0F * controlMCUSlugsMKII_B.Product[6]; 1063 1064 /* Saturate: '<S318>/[-1,1]' */ 1065 controlMCUSlugsMKII_B.u1 = rt_SATURATE(rtb_Abs2, -9.990000129E-001F, 1066 9.998999834E-001F); 1067 1068 /* S-Function "dsPIC_C_function_Call" Block: <S318>/C Function Call */ 1069 controlMCUSlugsMKII_B.CFunctionCall_c = myAsin(controlMCUSlugsMKII_B.u1); 1070 1071 /* S-Function "dsPIC_C_function_Call" Block: <S316>/C Function Call */ 1072 controlMCUSlugsMKII_B.CFunctionCall_c5 = myAtan2(controlMCUSlugsMKII_B.Product 1073 [3],controlMCUSlugsMKII_B.Product[0]); 1074 1075 /* Embedded MATLAB: '<S9>/Embedded MATLAB Function' */ 1076 if ((real_T)controlMCUSlugsMKII_B.CFunctionCall_c5 < 0.0) { 1077 /* This block supports an embeddable subset of the MATLAB language. */ 1078 /* See the help menu for details. */ 1079 rtb_y_d = controlMCUSlugsMKII_B.CFunctionCall_c5 + 6.283185482E+000F; 1080 } else { 1081 rtb_y_d = controlMCUSlugsMKII_B.CFunctionCall_c5; 1082 } 1083 1084 /* Stateflow: '<S9>/myMux Fun1' */ 1085 controlMCUSlugs_myMuxFun1_o(controlMCUSlugsMKII_B.CFunctionCall_i0, 1086 controlMCUSlugsMKII_B.CFunctionCall_c, rtb_y_d, 1087 &controlMCUSlugsMKII_B.sf_myMuxFun1_e); 1088 1089 { 1090 static const int_T dims[3] = { 3, 3, 1 }; 1091 1092 rt_MatMultRR_Sgl(controlMCUSlugsMKII_B.Product1, rtb_MathFunction, 1093 &controlMCUSlugsMKII_B.GetAttitudenavSupportc[3], &dims[0]); 1094 } 1095 1096 /* Embedded MATLAB: '<S9>/myMux Fun2' */ 1097 { 1098 int16_T eml_i0; 1099 1100 /* This block supports an embeddable subset of the MATLAB language. */ 1101 /* See the help menu for details. */ 1102 for (eml_i0 = 0; eml_i0 < 3; eml_i0++) { 1103 controlMCUSlugsMKII_B.y_n[eml_i0] = 1104 controlMCUSlugsMKII_B.sf_myMuxFun1_e.y_i[eml_i0]; 1105 } 1106 1107 for (eml_i0 = 0; eml_i0 < 3; eml_i0++) { 1108 controlMCUSlugsMKII_B.y_n[eml_i0 + 3] = controlMCUSlugsMKII_B.Product1 1109 [eml_i0]; 1110 } 1111 } 1112 1113 /* S-Function "dsPIC_C_function_Call" Block: <S3>/Get Pid according to Index2 */ 1114 getPidIdx(((uint8_T)2U),&controlMCUSlugsMKII_B.GetPidaccordingtoIndex2[0]); 1115 1116 /* S-Function "dsPIC_C_function_Call" Block: <Root>/wp Fly? [navSupport.c] */ 1117 controlMCUSlugsMKII_B.wpFlynavSupportc = isWpFly(); 1118 1119 /* S-Function "dsPIC_C_function_Call" Block: <Root>/Get Max WP [navSupport.c] */ 1120 controlMCUSlugsMKII_B.GetMaxWPnavSupportc = getMaxWp(); 1121 1122 /* S-Function "dsPIC_C_function_Call" Block: <Root>/Get Vned [navSupport.c] */ 1123 getVned(&controlMCUSlugsMKII_B.GetVnednavSupportc[0]); 1124 1125 /* S-Function "dsPIC_C_function_Call" Block: <Root>/[navSupport.c] */ 1126 getPidIdx(((uint8_T)5U),&controlMCUSlugsMKII_B.navSupportc_o[0]); 1127 1128 /* Output and update for atomic system: '<S2>/Navigation' */ 1129 1130 /* Saturate: '<S260>/[-25 25]' */ 1131 rtb_u525[0] = rt_SATURATE(controlMCUSlugsMKII_B.GetVnednavSupportc[0], -25.0F, 1132 25.0F); 1133 rtb_u525[1] = rt_SATURATE(controlMCUSlugsMKII_B.GetVnednavSupportc[1], -25.0F, 1134 25.0F); 1135 rtb_u525[2] = rt_SATURATE(controlMCUSlugsMKII_B.GetVnednavSupportc[2], -25.0F, 1136 25.0F); 1137 1138 /* Embedded MATLAB: '<S263>/Embedded MATLAB Function' */ 1139 1140 /* This block supports an embeddable subset of the MATLAB language. */ 1141 /* See the help menu for details. */ 1142 rtb_xDoty = rtb_u525[0] * rtb_u525[0] + rtb_u525[1] * rtb_u525[1]; 1143 1144 /* Stateflow: '<S264>/negprotect' */ 1145 controlMCUSlug_negprotect_e(rtb_xDoty, &controlMCUSlugsMKII_B.sf_negprotect_h); 1146 1147 /* S-Function "dsPIC_C_function_Call" Block: <S264>/C Function Call */ 1148 controlMCUSlugsMKII_B.CFunctionCall_a = mySqrt 1149 (controlMCUSlugsMKII_B.sf_negprotect_h.zpVal_k); 1150 1151 /* Stateflow: '<S239>/negprotect1' */ 1152 controlMCUSlugsM_negprotect(controlMCUSlugsMKII_B.CFunctionCall_a, 1153 &controlMCUSlugsMKII_B.sf_negprotect1); 1154 1155 /* RelationalOperator: '<S261>/Relational Operator2' */ 1156 rtb_RelationalOperator2_k = (controlMCUSlugsMKII_B.sf_negprotect1.zpVal == 1157 controlMCUSlugsMKII_B.sf_negprotect1.zpVal); 1158 1159 /* S-Function (sfix_udelay): '<S261>/Integer Delay3' 1160 * 1161 * Regarding '<S261>/Integer Delay3': 1162 * Integer/Tapped Delay Block: '<S261>/Integer Delay3' 1163 */ 1164 rtb_jxk = controlMCUSlugsMKII_DWork.IntegerDelay3_X_lj; 1165 1166 /* Switch: '<S261>/Switch3' */ 1167 if (rtb_RelationalOperator2_k > 0U) { 1168 rtb_Switch3_h = controlMCUSlugsMKII_B.sf_negprotect1.zpVal; 1169 } else { 1170 rtb_Switch3_h = rtb_jxk; 1171 } 1172 1173 /* Saturate: '<S239>/[0 45]' */ 1174 rtb_u45 = rt_SATURATE(rtb_Switch3_h, 0.0F, 45.0F); 1175 1176 /* Product: '<S239>/Divide4' */ 1177 rtb_Divide4 = controlMCUSlugsMKII_B.navSupportc_o[0] * rtb_u45; 1178 1179 /* Saturate: '<S237>/[-25 25]' */ 1180 rtb_u525_m[0] = rt_SATURATE(controlMCUSlugsMKII_B.GetVnednavSupportc[0], 1181 -25.0F, 25.0F); 1182 rtb_u525_m[1] = rt_SATURATE(controlMCUSlugsMKII_B.GetVnednavSupportc[1], 1183 -25.0F, 25.0F); 1184 rtb_u525_m[2] = rt_SATURATE(controlMCUSlugsMKII_B.GetVnednavSupportc[2], 1185 -25.0F, 25.0F); 1186 1187 /* S-Function (sfix_udelay): '<S124>/Integer Delay2' 1188 * 1189 * Regarding '<S124>/Integer Delay2': 1190 * Integer/Tapped Delay Block: '<S124>/Integer Delay2' 1191 */ 1192 rtb_IntegerDelay2 = controlMCUSlugsMKII_DWork.IntegerDelay2_X; 1193 1194 /* S-Function (sfix_udelay): '<S138>/Integer Delay1' 1195 * 1196 * Regarding '<S138>/Integer Delay1': 1197 * Integer/Tapped Delay Block: '<S138>/Integer Delay1' 1198 */ 1199 rtb_IntegerDelay1_oe = controlMCUSlugsMKII_DWork.IntegerDelay1_X_p; 1200 1201 { 1202 ZCEventType zcEvent; 1203 1204 /* InitialCondition: '<S124>/IC1' incorporates: 1205 * DataTypeConversion: '<S124>/Data Type Conversion1' 1206 */ 1207 if (controlMCUSlugsMKII_DWork.IC1_FirstOutputTime) { 1208 controlMCUSlugsMKII_DWork.IC1_FirstOutputTime = 0U; 1209 rtb_IC1 = 0.0; 1210 } else { 1211 rtb_IC1 = (real_T)controlMCUSlugsMKII_B.wpFlynavSupportc; 1212 } 1213 1214 /* HitCross: '<S138>/Hit Crossing1' */ 1215 zcEvent = rt_ZCFcn(RISING_ZERO_CROSSING, 1216 &controlMCUSlugsM_PrevZCSigState.HitCrossing1_ZCE, 1217 (rtb_IC1 - 0.3)); 1218 if (controlMCUSlugsMKII_DWork.HitCrossing1_MODE == 0L) { 1219 if (zcEvent != NO_ZCEVENT) { 1220 controlMCUSlugsMKII_B.HitCrossing1 = 1221 ((controlMCUSlugsMKII_B.HitCrossing1 == 1 ? 0 : 1) != 0); 1222 controlMCUSlugsMKII_DWork.HitCrossing1_MODE = 1L; 1223 } else if ((controlMCUSlugsMKII_B.HitCrossing1 == 1) && (rtb_IC1 != 0.3)) 1224 { 1225 controlMCUSlugsMKII_B.HitCrossing1 = 0U; 1226 } 1227 } else { 1228 if (rtb_IC1 != 0.3) { 1229 controlMCUSlugsMKII_B.HitCrossing1 = 0U; 1230 } 1231 1232 controlMCUSlugsMKII_DWork.HitCrossing1_MODE = 0L; 1233 } 1234 1235 /* Sum: '<S138>/Add1' */ 1236 rtb_Add1_i = (uint8_T)((uint16_T)rtb_IntegerDelay1_oe + (uint16_T) 1237 controlMCUSlugsMKII_B.HitCrossing1); 1238 1239 /* Embedded MATLAB: '<S124>/Embedded MATLAB Function' incorporates: 1240 * Constant: '<Root>/Constant1' 1241 */ 1242 { 1243 uint16_T eml_u0; 1244 uint8_T eml_u1; 1245 uint16_T eml_u2; 1246 uint8_T eml_u3; 1247 uint16_T eml_u4; 1248 uint8_T eml_u5; 1249 uint16_T eml_u6; 1250 uint8_T eml_u7; 1251 1252 /* Declare static variables */ 1253 if (!controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_g.fromWP_not_empty) 1254 { 1255 /* initialize the static variables */ 1256 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_g.fromWP = ((uint8_T)1U); 1257 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_g.fromWP_not_empty = 1258 true; 1259 eml_u0 = (uint16_T) 1260 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_g.fromWP + 1U; 1261 if (eml_u0 > 255U) { 1262 eml_u1 = MAX_uint8_T; 1263 } else { 1264 eml_u1 = (uint8_T)eml_u0; 1265 } 1266 1267 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_g.toWP = eml_u1; 1268 eml_u2 = (uint16_T) 1269 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_g.toWP + 1U; 1270 if (eml_u2 > 255U) { 1271 eml_u3 = MAX_uint8_T; 1272 } else { 1273 eml_u3 = (uint8_T)eml_u2; 1274 } 1275 1276 if (controlMCUSlugsMKII_B.GetMaxWPnavSupportc > eml_u3) { 1277 /* check if there are more than 2 WPs */ 1278 eml_u4 = (uint16_T) 1279 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_g.toWP + 1U; 1280 if (eml_u4 > 255U) { 1281 eml_u5 = MAX_uint8_T; 1282 } else { 1283 eml_u5 = (uint8_T)eml_u4; 1284 } 1285 1286 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_g.totoWP = eml_u5; 1287 } else { 1288 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_g.totoWP = 1289 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_g.fromWP; 1290 } 1291 } 1292 1293 if (rtb_Add1_i == 1) { 1294 /* if WP Nav was just enabled */ 1295 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_g.fromWP = 0U; 1296 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_g.toWP = 1U; 1297 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_g.totoWP = 2U; 1298 } 1299 1300 if (rtb_IntegerDelay2 == 1.0) { 1301 /* if there is a change of wp */ 1302 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_g.fromWP = 1303 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_g.toWP; 1304 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_g.toWP = 1305 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_g.totoWP; 1306 eml_u6 = (uint16_T) 1307 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_g.totoWP + 1U; 1308 if (eml_u6 > 255U) { 1309 eml_u7 = MAX_uint8_T; 1310 } else { 1311 eml_u7 = (uint8_T)eml_u6; 1312 } 1313 1314 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_g.totoWP = eml_u7; 1315 if (controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_g.totoWP > 1316 controlMCUSlugsMKII_B.GetMaxWPnavSupportc) { 1317 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_g.totoWP = ((uint8_T) 1318 1U); 1319 } 1320 } 1321 1322 controlMCUSlugsMKII_B.wp0 = 1323 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_g.fromWP; 1324 controlMCUSlugsMKII_B.wp1 = 1325 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_g.toWP; 1326 controlMCUSlugsMKII_B.wp2 = 1327 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_g.totoWP; 1328 } 1329 1330 if (rtb_Add1_i > 0U) { 1331 /* Output and update for enable system: '<S124>/Grab IC' */ 1332 1333 /* Inport: '<S136>/UAV Pos' */ 1334 controlMCUSlugsMKII_B.UAVPos[0] = controlMCUSlugsMKII_B.GetXYZnavSupportc 1335 [0]; 1336 controlMCUSlugsMKII_B.UAVPos[1] = controlMCUSlugsMKII_B.GetXYZnavSupportc 1337 [1]; 1338 controlMCUSlugsMKII_B.UAVPos[2] = controlMCUSlugsMKII_B.GetXYZnavSupportc 1339 [2]; 1340 1341 /* S-Function "dsPIC_C_function_Call" Block: <S174>/C Function Call */ 1342 getWP(((uint8_T)11U),&controlMCUSlugsMKII_B.CFunctionCall_aj[0]); 1343 1344 /* Gain: '<S175>/Deg2R' */ 1345 rtb_Deg2R_i = 1.745329238E-002F * controlMCUSlugsMKII_B.CFunctionCall_aj[0]; 1346 1347 /* Trigonometry: '<S175>/sin(phi)' */ 1348 rtb_sinphi_m = (real32_T)sin(rtb_Deg2R_i); 1349 1350 /* Product: '<S175>/Rh' incorporates: 1351 * Constant: '<S175>/Re=equatorial radius' 1352 * Constant: '<S175>/const' 1353 * Fcn: '<S175>/f' 1354 * Product: '<S175>/Product1' 1355 * Product: '<S175>/sin(phi)^2' 1356 * Sum: '<S175>/Sum1' 1357 */ 1358 rtb_Rh_i = 6.378137E+006F / (real32_T)sqrt(1.0F - rtb_sinphi_m * 1359 rtb_sinphi_m * controlMCUSlugsMKII_ConstB.Sum5_lj); 1360 1361 /* Sum: '<S175>/Sum2' */ 1362 rtb_Sum2_jc = controlMCUSlugsMKII_B.CFunctionCall_aj[2] + rtb_Rh_i; 1363 1364 /* Trigonometry: '<S175>/cos(phi)' */ 1365 rtb_Deg2R_i = (real32_T)cos(rtb_Deg2R_i); 1366 1367 /* Gain: '<S175>/Deg2R1' */ 1368 rtb_Deg2R1_d = 1.745329238E-002F * controlMCUSlugsMKII_B.CFunctionCall_aj 1369 [1]; 1370 1371 /* Product: '<S175>/(Rh+h)cos(phi)*cos(lamb)=Xe' incorporates: 1372 * Trigonometry: '<S175>/cos(lamb)' 1373 */ 1374 controlMCUSlugsMKII_B.RhhcosphicoslambXe = rtb_Sum2_jc * rtb_Deg2R_i * 1375 (real32_T)cos(rtb_Deg2R1_d); 1376 1377 /* Product: '<S175>/(Rh+h)cos(phi)*sin(lamb)=Ye' incorporates: 1378 * Trigonometry: '<S175>/sin(lamb)' 1379 */ 1380 controlMCUSlugsMKII_B.RhhcosphisinlambYe = rtb_Sum2_jc * rtb_Deg2R_i * 1381 (real32_T)sin(rtb_Deg2R1_d); 1382 1383 /* Product: '<S175>/Ze' incorporates: 1384 * Product: '<S175>/Rh(1-e^2)' 1385 * Sum: '<S175>/Sum4' 1386 */ 1387 controlMCUSlugsMKII_B.Ze = (controlMCUSlugsMKII_ConstB.e2_i * rtb_Rh_i + 1388 controlMCUSlugsMKII_B.CFunctionCall_aj[2]) * rtb_sinphi_m; 1389 } 1390 } 1391 1392 /* S-Function (sfix_udelay): '<S139>/Integer Delay2' 1393 * 1394 * Regarding '<S139>/Integer Delay2': 1395 * Integer/Tapped Delay Block: '<S139>/Integer Delay2' 1396 */ 1397 rtb_IntegerDelay2_b = controlMCUSlugsMKII_DWork.IntegerDelay2_X_l; 1398 1399 /* InitialCondition: '<S139>/IC' */ 1400 if (controlMCUSlugsMKII_DWork.IC_FirstOutputTime_i) { 1401 controlMCUSlugsMKII_DWork.IC_FirstOutputTime_i = 0U; 1402 controlMCUSlugsMKII_B.IC = 1.0; 1403 } else { 1404 controlMCUSlugsMKII_B.IC = rtb_IntegerDelay2; 1405 } 1406 1407 /* Logic: '<S124>/Logical Operator' incorporates: 1408 * Sum: '<S139>/Add' 1409 */ 1410 rtb_LogicalOperator_e = ((rtb_IntegerDelay2_b + controlMCUSlugsMKII_B.IC != 1411 0.0) || (rtb_Add1_i != 0U)); 1412 if (rtb_LogicalOperator_e) { 1413 /* Output and update for enable system: '<S16>/Get Wps Compute Frenet' */ 1414 1415 /* S-Function "dsPIC_C_function_Call" Block: <S208>/C Function Call */ 1416 getWP(controlMCUSlugsMKII_B.wp0,&controlMCUSlugsMKII_B.CFunctionCall_d[0]); 1417 1418 { 1419 real32_T cg_in[9]; 1420 int16_T i; 1421 real32_T rtb_Deg2R_idx; 1422 real32_T rtb_Deg2R_idx_0; 1423 1424 /* Switch: '<S125>/Switch' */ 1425 if (controlMCUSlugsMKII_B.wp0 > 0U) { 1426 { 1427 real32_T cg_in[9]; 1428 int16_T i; 1429 1430 /* Gain: '<S233>/Deg2R' */ 1431 rtb_Deg2R_g = 1.745329238E-002F * 1432 controlMCUSlugsMKII_B.CFunctionCall_d[0]; 1433 1434 /* Trigonometry: '<S233>/sin(phi)' */ 1435 rtb_sinphi_a = (real32_T)sin(rtb_Deg2R_g); 1436 1437 /* Product: '<S233>/Rh' incorporates: 1438 * Constant: '<S233>/Re=equatorial radius' 1439 * Constant: '<S233>/const' 1440 * Fcn: '<S233>/f' 1441 * Product: '<S233>/Product1' 1442 * Product: '<S233>/sin(phi)^2' 1443 * Sum: '<S233>/Sum1' 1444 */ 1445 rtb_Rh_o = 6.378137E+006F / (real32_T)sqrt(1.0F - rtb_sinphi_a * 1446 rtb_sinphi_a * controlMCUSlugsMKII_ConstB.Sum5); 1447 1448 /* Gain: '<S233>/Deg2R1' */ 1449 rtb_Deg2R1_j = 1.745329238E-002F * 1450 controlMCUSlugsMKII_B.CFunctionCall_d[1]; 1451 1452 /* Trigonometry: '<S233>/cos(phi)' */ 1453 rtb_Deg2R_g = (real32_T)cos(rtb_Deg2R_g); 1454 1455 /* Sum: '<S233>/Sum2' */ 1456 rtb_Sum2_j = controlMCUSlugsMKII_B.CFunctionCall_d[2] + rtb_Rh_o; 1457 1458 /* Sum: '<S207>/Sum1' incorporates: 1459 * Product: '<S233>/(Rh+h)cos(phi)*cos(lamb)=Xe' 1460 * Product: '<S233>/(Rh+h)cos(phi)*sin(lamb)=Ye' 1461 * Product: '<S233>/Rh(1-e^2)' 1462 * Product: '<S233>/Ze' 1463 * Sum: '<S233>/Sum4' 1464 * Trigonometry: '<S233>/cos(lamb)' 1465 * Trigonometry: '<S233>/sin(lamb)' 1466 */ 1467 rtb_Sum1_f[0] = rtb_Sum2_j * rtb_Deg2R_g * (real32_T)cos(rtb_Deg2R1_j) 1468 - controlMCUSlugsMKII_B.RhhcosphicoslambXe; 1469 rtb_Sum1_f[1] = rtb_Sum2_j * rtb_Deg2R_g * (real32_T)sin(rtb_Deg2R1_j) 1470 - controlMCUSlugsMKII_B.RhhcosphisinlambYe; 1471 rtb_Sum1_f[2] = (controlMCUSlugsMKII_ConstB.e2 * rtb_Rh_o + 1472 controlMCUSlugsMKII_B.CFunctionCall_d[2]) * 1473 rtb_sinphi_a - controlMCUSlugsMKII_B.Ze; 1474 1475 /* Gain: '<S232>/Deg2R' */ 1476 rtb_Deg2R_g = 1.745329238E-002F * 1477 controlMCUSlugsMKII_B.CFunctionCall_aj[1]; 1478 rtb_sinphi_a = 1.745329238E-002F * 1479 controlMCUSlugsMKII_B.CFunctionCall_aj[0]; 1480 1481 /* SignalConversion: '<S232>/TmpHiddenBufferAtProduct1Inport1' incorporates: 1482 * Fcn: '<S235>/11' 1483 * Fcn: '<S235>/12' 1484 * Fcn: '<S235>/13' 1485 * Fcn: '<S235>/21' 1486 * Fcn: '<S235>/22' 1487 * Fcn: '<S235>/23' 1488 * Fcn: '<S235>/31' 1489 * Fcn: '<S235>/32' 1490 * Fcn: '<S235>/33' 1491 */ 1492 cg_in[0] = (real32_T)(cos(rtb_Deg2R_g) * cos(rtb_sinphi_a)); 1493 cg_in[1] = (real32_T)(-sin(rtb_Deg2R_g)); 1494 cg_in[2] = (real32_T)((-sin(rtb_sinphi_a)) * cos(rtb_Deg2R_g)); 1495 cg_in[3] = (real32_T)(sin(rtb_Deg2R_g) * cos(rtb_sinphi_a)); 1496 cg_in[4] = (real32_T)cos(rtb_Deg2R_g); 1497 cg_in[5] = (real32_T)((-sin(rtb_Deg2R_g)) * sin(rtb_sinphi_a)); 1498 cg_in[6] = (real32_T)sin(rtb_sinphi_a); 1499 cg_in[7] = 0.0F; 1500 cg_in[8] = (real32_T)cos(rtb_sinphi_a); 1501 for (i = 0; i < 9; i++) { 1502 rtb_TmpHiddenBufferAtProduct1_h[i] = cg_in[i]; 1503 } 1504 } 1505 1506 { 1507 static const int_T dims[3] = { 3, 3, 1 }; 1508 1509 rt_MatMultRR_Sgl(rtb_Product1_jw, rtb_TmpHiddenBufferAtProduct1_h, 1510 rtb_Sum1_f, &dims[0]); 1511 } 1512 1513 /* Gain Block: '<S207>/UEN 2 NEU' 1514 * About '<S207>/UEN 2 NEU :' 1515 * Gain value: [0 0 1;0 1 0;1 0 0] 1516 1517 1518 */ 1519 { 1520 static const int_T dims[3] = { 3, 3, 1 }; 1521 1522 rt_MatMultRR_Sgl((real32_T *)rtb_UEN2NEU, (real32_T *) 1523 &controlMCUSlugsMKII_ConstP.pooled51[0], 1524 (real32_T *)rtb_Product1_jw, &dims[0]); 1525 } 1526 1527 controlMCUSlugsMKII_B.Switch[0] = rtb_UEN2NEU[0]; 1528 controlMCUSlugsMKII_B.Switch[1] = rtb_UEN2NEU[1]; 1529 controlMCUSlugsMKII_B.Switch[2] = rtb_UEN2NEU[2]; 1530 } else { 1531 controlMCUSlugsMKII_B.Switch[0] = controlMCUSlugsMKII_B.UAVPos[0]; 1532 controlMCUSlugsMKII_B.Switch[1] = controlMCUSlugsMKII_B.UAVPos[1]; 1533 controlMCUSlugsMKII_B.Switch[2] = controlMCUSlugsMKII_B.UAVPos[2]; 1534 } 1535 1536 /* Gain: '<S227>/Deg2R' */ 1537 rtb_Deg2R_idx = 1.745329238E-002F * controlMCUSlugsMKII_B.CFunctionCall_aj 1538 [1]; 1539 rtb_Deg2R_idx_0 = 1.745329238E-002F * 1540 controlMCUSlugsMKII_B.CFunctionCall_aj[0]; 1541 1542 /* SignalConversion: '<S227>/TmpHiddenBufferAtProduct1Inport1' incorporates: 1543 * Fcn: '<S230>/11' 1544 * Fcn: '<S230>/12' 1545 * Fcn: '<S230>/13' 1546 * Fcn: '<S230>/21' 1547 * Fcn: '<S230>/22' 1548 * Fcn: '<S230>/23' 1549 * Fcn: '<S230>/31' 1550 * Fcn: '<S230>/32' 1551 * Fcn: '<S230>/33' 1552 */ 1553 cg_in[0] = (real32_T)(cos(rtb_Deg2R_idx) * cos(rtb_Deg2R_idx_0)); 1554 cg_in[1] = (real32_T)(-sin(rtb_Deg2R_idx)); 1555 cg_in[2] = (real32_T)((-sin(rtb_Deg2R_idx_0)) * cos(rtb_Deg2R_idx)); 1556 cg_in[3] = (real32_T)(sin(rtb_Deg2R_idx) * cos(rtb_Deg2R_idx_0)); 1557 cg_in[4] = (real32_T)cos(rtb_Deg2R_idx); 1558 cg_in[5] = (real32_T)((-sin(rtb_Deg2R_idx)) * sin(rtb_Deg2R_idx_0)); 1559 cg_in[6] = (real32_T)sin(rtb_Deg2R_idx_0); 1560 cg_in[7] = 0.0F; 1561 cg_in[8] = (real32_T)cos(rtb_Deg2R_idx_0); 1562 for (i = 0; i < 9; i++) { 1563 rtb_TmpHiddenBufferAtProduct1In[i] = cg_in[i]; 1564 } 1565 } 1566 1567 /* S-Function "dsPIC_C_function_Call" Block: <S209>/C Function Call */ 1568 getWP(controlMCUSlugsMKII_B.wp1,&controlMCUSlugsMKII_B.CFunctionCall_a1[0]); 1569 1570 /* Gain: '<S228>/Deg2R' */ 1571 rtb_Deg2R_c = 1.745329238E-002F * controlMCUSlugsMKII_B.CFunctionCall_a1[0]; 1572 1573 /* Trigonometry: '<S228>/sin(phi)' */ 1574 rtb_sinphi = (real32_T)sin(rtb_Deg2R_c); 1575 1576 /* Product: '<S228>/Rh' incorporates: 1577 * Constant: '<S228>/Re=equatorial radius' 1578 * Constant: '<S228>/const' 1579 * Fcn: '<S228>/f' 1580 * Product: '<S228>/Product1' 1581 * Product: '<S228>/sin(phi)^2' 1582 * Sum: '<S228>/Sum1' 1583 */ 1584 rtb_Rh = 6.378137E+006F / (real32_T)sqrt(1.0F - rtb_sinphi * rtb_sinphi * 1585 controlMCUSlugsMKII_ConstB.Sum5_l); 1586 1587 /* Sum: '<S228>/Sum2' */ 1588 rtb_Sum2_d = controlMCUSlugsMKII_B.CFunctionCall_a1[2] + rtb_Rh; 1589 1590 /* Trigonometry: '<S228>/cos(phi)' */ 1591 rtb_Deg2R_c = (real32_T)cos(rtb_Deg2R_c); 1592 1593 /* Gain: '<S228>/Deg2R1' */ 1594 rtb_Deg2R1 = 1.745329238E-002F * controlMCUSlugsMKII_B.CFunctionCall_a1[1]; 1595 1596 /* Sum: '<S206>/Sum1' incorporates: 1597 * Product: '<S228>/(Rh+h)cos(phi)*cos(lamb)=Xe' 1598 * Product: '<S228>/(Rh+h)cos(phi)*sin(lamb)=Ye' 1599 * Product: '<S228>/Rh(1-e^2)' 1600 * Product: '<S228>/Ze' 1601 * Sum: '<S228>/Sum4' 1602 * Trigonometry: '<S228>/cos(lamb)' 1603 * Trigonometry: '<S228>/sin(lamb)' 1604 */ 1605 rtb_UEN2NEU_f[0] = rtb_Sum2_d * rtb_Deg2R_c * (real32_T)cos(rtb_Deg2R1) - 1606 controlMCUSlugsMKII_B.RhhcosphicoslambXe; 1607 rtb_UEN2NEU_f[1] = rtb_Sum2_d * rtb_Deg2R_c * (real32_T)sin(rtb_Deg2R1) - 1608 controlMCUSlugsMKII_B.RhhcosphisinlambYe; 1609 rtb_UEN2NEU_f[2] = (controlMCUSlugsMKII_ConstB.e2_c * rtb_Rh + 1610 controlMCUSlugsMKII_B.CFunctionCall_a1[2]) * rtb_sinphi 1611 - controlMCUSlugsMKII_B.Ze; 1612 1613 { 1614 static const int_T dims[3] = { 3, 3, 1 }; 1615 1616 rt_MatMultRR_Sgl(rtb_Product1_az, rtb_TmpHiddenBufferAtProduct1In, 1617 rtb_UEN2NEU_f, &dims[0]); 1618 } 1619 1620 /* Gain Block: '<S206>/UEN 2 NEU' 1621 * About '<S206>/UEN 2 NEU :' 1622 * Gain value: [0 0 1;0 1 0;1 0 0] 1623 1624 1625 */ 1626 { 1627 static const int_T dims[3] = { 3, 3, 1 }; 1628 1629 rt_MatMultRR_Sgl((real32_T *)rtb_UEN2NEU_f, (real32_T *) 1630 &controlMCUSlugsMKII_ConstP.pooled51[0], 1631 (real32_T *)rtb_Product1_az, &dims[0]); 1632 } 1633 1634 /* Reshape: '<S206>/Reshape1' */ 1635 controlMCUSlugsMKII_B.Reshape1[0] = rtb_UEN2NEU_f[0]; 1636 controlMCUSlugsMKII_B.Reshape1[1] = rtb_UEN2NEU_f[1]; 1637 controlMCUSlugsMKII_B.Reshape1[2] = rtb_UEN2NEU_f[2]; 1638 1639 /* Sum: '<S205>/Add' */ 1640 rtb_Add_fn[0] = controlMCUSlugsMKII_B.Reshape1[0] - 1641 controlMCUSlugsMKII_B.Switch[0]; 1642 rtb_Add_fn[1] = controlMCUSlugsMKII_B.Reshape1[1] - 1643 controlMCUSlugsMKII_B.Switch[1]; 1644 rtb_Add_fn[2] = controlMCUSlugsMKII_B.Reshape1[2] - 1645 controlMCUSlugsMKII_B.Switch[2]; 1646 1647 /* Stateflow: '<S215>/Embedded MATLAB Function' */ 1648 co_EmbeddedMATLABFunction_h(rtb_Add_fn, 1649 &controlMCUSlugsMKII_B.sf_EmbeddedMATLABFunction_hk); 1650 1651 /* Stateflow: '<S214>/negprotect' */ 1652 controlMCUSlug_negprotect_e 1653 (controlMCUSlugsMKII_B.sf_EmbeddedMATLABFunction_hk.xDoty_j, 1654 &controlMCUSlugsMKII_B.sf_negprotect_g); 1655 1656 /* S-Function "dsPIC_C_function_Call" Block: <S214>/C Function Call */ 1657 controlMCUSlugsMKII_B.CFunctionCall_b = mySqrt 1658 (controlMCUSlugsMKII_B.sf_negprotect_g.zpVal_k); 1659 1660 { 1661 real32_T rtmax_U0DataInY0Container; 1662 1663 /* Saturate: '<S210>/Zero Bound' */ 1664 rtmax_U0DataInY0Container = rt_MAX(controlMCUSlugsMKII_B.CFunctionCall_b, 1665 1.000000047E-003F); 1666 1667 /* Product: '<S210>/Divide' */ 1668 controlMCUSlugsMKII_B.Divide_n[0] = rtb_Add_fn[0] / 1669 rtmax_U0DataInY0Container; 1670 controlMCUSlugsMKII_B.Divide_n[1] = rtb_Add_fn[1] / 1671 rtmax_U0DataInY0Container; 1672 controlMCUSlugsMKII_B.Divide_n[2] = rtb_Add_fn[2] / 1673 rtmax_U0DataInY0Container; 1674 1675 /* Embedded MATLAB: '<S205>/Select N Terms' */ 1676 1677 /* This block supports an embeddable subset of the MATLAB language. */ 1678 /* See the help menu for details. */ 1679 controlMCUSlugsMKII_B.N[0] = -controlMCUSlugsMKII_B.Divide_n[1]; 1680 controlMCUSlugsMKII_B.N[1] = controlMCUSlugsMKII_B.Divide_n[0]; 1681 controlMCUSlugsMKII_B.N[2] = 0.0F; 1682 } 1683 1684 /* Stateflow: '<S222>/Embedded MATLAB Function' */ 1685 co_EmbeddedMATLABFunction_h(controlMCUSlugsMKII_B.N, 1686 &controlMCUSlugsMKII_B.sf_EmbeddedMATLABFunction_or); 1687 1688 /* Stateflow: '<S221>/negprotect' */ 1689 controlMCUSlug_negprotect_e 1690 (controlMCUSlugsMKII_B.sf_EmbeddedMATLABFunction_or.xDoty_j, 1691 &controlMCUSlugsMKII_B.sf_negprotect_bp); 1692 1693 /* S-Function "dsPIC_C_function_Call" Block: <S221>/C Function Call */ 1694 controlMCUSlugsMKII_B.CFunctionCall_c0 = mySqrt 1695 (controlMCUSlugsMKII_B.sf_negprotect_bp.zpVal_k); 1696 1697 { 1698 real32_T rtmax_U0DataInY0Container; 1699 1700 /* Saturate: '<S211>/Zero Bound' */ 1701 rtmax_U0DataInY0Container = rt_MAX(controlMCUSlugsMKII_B.CFunctionCall_c0, 1702 1.000000047E-003F); 1703 1704 /* Product: '<S211>/Divide' */ 1705 controlMCUSlugsMKII_B.Divide_p[0] = controlMCUSlugsMKII_B.N[0] / 1706 rtmax_U0DataInY0Container; 1707 controlMCUSlugsMKII_B.Divide_p[1] = controlMCUSlugsMKII_B.N[1] / 1708 rtmax_U0DataInY0Container; 1709 controlMCUSlugsMKII_B.Divide_p[2] = controlMCUSlugsMKII_B.N[2] / 1710 rtmax_U0DataInY0Container; 1711 } 1712 } 1713 1714 /* Sum: '<S123>/Add' */ 1715 rtb_Add_id[0] = controlMCUSlugsMKII_B.Reshape1[0] - 1716 controlMCUSlugsMKII_B.GetXYZnavSupportc[0]; 1717 rtb_Add_id[1] = controlMCUSlugsMKII_B.Reshape1[1] - 1718 controlMCUSlugsMKII_B.GetXYZnavSupportc[1]; 1719 rtb_Add_id[2] = controlMCUSlugsMKII_B.Reshape1[2] - 1720 controlMCUSlugsMKII_B.GetXYZnavSupportc[2]; 1721 1722 /* Stateflow: '<S129>/Embedded MATLAB Function' */ 1723 cont_EmbeddedMATLABFunction(controlMCUSlugsMKII_B.Divide_p, rtb_Add_id, 1724 &controlMCUSlugsMKII_B.sf_EmbeddedMATLABFunction_m); 1725 1726 /* RelationalOperator: '<S132>/Relational Operator2' */ 1727 rtb_RelationalOperator2_g = 1728 (controlMCUSlugsMKII_B.sf_EmbeddedMATLABFunction_m.xDoty == 1729 controlMCUSlugsMKII_B.sf_EmbeddedMATLABFunction_m.xDoty); 1730 1731 /* S-Function (sfix_udelay): '<S132>/Integer Delay3' 1732 * 1733 * Regarding '<S132>/Integer Delay3': 1734 * Integer/Tapped Delay Block: '<S132>/Integer Delay3' 1735 */ 1736 rtb_jxk = controlMCUSlugsMKII_DWork.IntegerDelay3_X_b3; 1737 1738 /* Switch: '<S132>/Switch3' */ 1739 if (rtb_RelationalOperator2_g > 0U) { 1740 rtb_Switch3_nn = controlMCUSlugsMKII_B.sf_EmbeddedMATLABFunction_m.xDoty; 1741 } else { 1742 rtb_Switch3_nn = rtb_jxk; 1743 } 1744 1745 /* Gain: '<S238>/Gain' */ 1746 rtb_Gain_m = -1.0F * rtb_Switch3_nn; 1747 1748 /* Product: '<S238>/Divide2' */ 1749 rtb_NPey[0] = controlMCUSlugsMKII_B.Divide_p[0] * rtb_Gain_m; 1750 rtb_NPey[1] = controlMCUSlugsMKII_B.Divide_p[1] * rtb_Gain_m; 1751 rtb_NPey[2] = controlMCUSlugsMKII_B.Divide_p[2] * rtb_Gain_m; 1752 1753 /* Sum: '<S238>/Add' incorporates: 1754 * Product: '<S238>/Divide' 1755 * Product: '<S238>/Divide1' 1756 */ 1757 controlMCUSlugsMKII_B.Add = rtb_Divide4 * rtb_Divide4 - rtb_Switch3_nn * 1758 rtb_Switch3_nn; 1759 1760 /* S-Function "dsPIC_C_function_Call" Block: <S256>/C Function Call */ 1761 controlMCUSlugsMKII_B.CFunctionCall_o = myAbs(controlMCUSlugsMKII_B.Add); 1762 1763 /* Stateflow: '<S255>/negprotect' */ 1764 controlMCUSlug_negprotect_e(controlMCUSlugsMKII_B.CFunctionCall_o, 1765 &controlMCUSlugsMKII_B.sf_negprotect_f); 1766 1767 /* S-Function "dsPIC_C_function_Call" Block: <S255>/C Function Call */ 1768 controlMCUSlugsMKII_B.CFunctionCall_f = mySqrt 1769 (controlMCUSlugsMKII_B.sf_negprotect_f.zpVal_k); 1770 1771 /* Sum: '<S238>/Add2' incorporates: 1772 * Product: '<S238>/Divide3' 1773 */ 1774 controlMCUSlugsMKII_B.L1_vec[0] = controlMCUSlugsMKII_B.Divide_n[0] * 1775 controlMCUSlugsMKII_B.CFunctionCall_f - rtb_NPey[0]; 1776 controlMCUSlugsMKII_B.L1_vec[1] = controlMCUSlugsMKII_B.Divide_n[1] * 1777 controlMCUSlugsMKII_B.CFunctionCall_f - rtb_NPey[1]; 1778 controlMCUSlugsMKII_B.L1_vec[2] = controlMCUSlugsMKII_B.Divide_n[2] * 1779 controlMCUSlugsMKII_B.CFunctionCall_f - rtb_NPey[2]; 1780 1781 /* Sum: '<S241>/Sum' incorporates: 1782 * Product: '<S245>/i x j' 1783 * Product: '<S245>/j x k' 1784 * Product: '<S245>/k x i' 1785 * Product: '<S246>/i x k' 1786 * Product: '<S246>/j x i' 1787 * Product: '<S246>/k x j' 1788 */ 1789 rtb_Sum_mb[0] = rtb_u525_m[1] * controlMCUSlugsMKII_B.L1_vec[2] - rtb_u525_m[2] 1790 * controlMCUSlugsMKII_B.L1_vec[1]; 1791 rtb_Sum_mb[1] = rtb_u525_m[2] * controlMCUSlugsMKII_B.L1_vec[0] - rtb_u525_m[0] 1792 * controlMCUSlugsMKII_B.L1_vec[2]; 1793 rtb_Sum_mb[2] = rtb_u525_m[0] * controlMCUSlugsMKII_B.L1_vec[1] - rtb_u525_m[1] 1794 * controlMCUSlugsMKII_B.L1_vec[0]; 1795 1796 /* Stateflow: '<S248>/Embedded MATLAB Function' */ 1797 co_EmbeddedMATLABFunction_h(controlMCUSlugsMKII_B.L1_vec, 1798 &controlMCUSlugsMKII_B.sf_EmbeddedMATLABFunction_d); 1799 1800 /* Stateflow: '<S247>/negprotect' */ 1801 controlMCUSlug_negprotect_e 1802 (controlMCUSlugsMKII_B.sf_EmbeddedMATLABFunction_d.xDoty_j, 1803 &controlMCUSlugsMKII_B.sf_negprotect_b); 1804 1805 /* S-Function "dsPIC_C_function_Call" Block: <S247>/C Function Call */ 1806 controlMCUSlugsMKII_B.CFunctionCall_m = mySqrt 1807 (controlMCUSlugsMKII_B.sf_negprotect_b.zpVal_k); 1808 1809 /* Stateflow: '<S237>/negprotect' */ 1810 controlMCUSlugsM_negprotect(controlMCUSlugsMKII_B.CFunctionCall_m, 1811 &controlMCUSlugsMKII_B.sf_negprotect_fm); 1812 1813 /* Product: '<S237>/Divide5' */ 1814 rtb_Divide5[0] = rtb_Sum_mb[0] / controlMCUSlugsMKII_B.sf_negprotect_fm.zpVal / 1815 rtb_u45; 1816 rtb_Divide5[1] = rtb_Sum_mb[1] / controlMCUSlugsMKII_B.sf_negprotect_fm.zpVal / 1817 rtb_u45; 1818 rtb_Divide5[2] = rtb_Sum_mb[2] / controlMCUSlugsMKII_B.sf_negprotect_fm.zpVal / 1819 rtb_u45; 1820 1821 /* S-Function (sfix_udelay): '<S254>/Integer Delay3' 1822 * 1823 * Regarding '<S254>/Integer Delay3': 1824 * Integer/Tapped Delay Block: '<S254>/Integer Delay3' 1825 */ 1826 rtb_IntegerDelay3_d = controlMCUSlugsMKII_DWork.IntegerDelay3_X_g; 1827 1828 /* Switch: '<S254>/Switch3' incorporates: 1829 * RelationalOperator: '<S254>/Relational Operator2' 1830 */ 1831 if (((uint8_T)(rtb_Divide5[2] == rtb_Divide5[2])) > 0U) { 1832 rtb_Switch3_a = rtb_Divide5[2]; 1833 } else { 1834 rtb_Switch3_a = rtb_IntegerDelay3_d; 1835 } 1836 1837 /* Saturate: '<S243>/[-1,1]' */ 1838 controlMCUSlugsMKII_B.u1_h = rt_SATURATE(rtb_Switch3_a, -9.990000129E-001F, 1839 9.998999834E-001F); 1840 1841 /* S-Function "dsPIC_C_function_Call" Block: <S243>/C Function Call */ 1842 controlMCUSlugsMKII_B.CFunctionCall_h = myAsin(controlMCUSlugsMKII_B.u1_h); 1843 1844 /* S-Function "dsPIC_C_function_Call" Block: <S269>/C Function Call */ 1845 controlMCUSlugsMKII_B.CFunctionCall_e = mySin 1846 (controlMCUSlugsMKII_B.CFunctionCall_h); 1847 1848 /* Product: '<S240>/Product' incorporates: 1849 * Constant: '<S240>/Constant' 1850 */ 1851 rtb_Product_m = (real32_T)((real_T)(rtb_u45 * rtb_u45) * 2.0) / rtb_Divide4 * 1852 controlMCUSlugsMKII_B.CFunctionCall_e; 1853 1854 /* S-Function (sfix_udelay): '<S268>/Integer Delay3' 1855 * 1856 * Regarding '<S268>/Integer Delay3': 1857 * Integer/Tapped Delay Block: '<S268>/Integer Delay3' 1858 */ 1859 rtb_IntegerDelay3_d = controlMCUSlugsMKII_DWork.IntegerDelay3_X_f; 1860 1861 /* Switch: '<S268>/Switch3' incorporates: 1862 * RelationalOperator: '<S268>/Relational Operator2' 1863 */ 1864 if (((uint8_T)(rtb_Product_m == rtb_Product_m)) > 0U) { 1865 rtb_Switch3_m = rtb_Product_m; 1866 } else { 1867 rtb_Switch3_m = rtb_IntegerDelay3_d; 1868 } 1869 1870 /* Product: '<S126>/Product' incorporates: 1871 * Saturate: '<S240>/[-25,25]' 1872 */ 1873 rtb_Product_c = controlMCUSlugsMKII_B.navSupportc_o[2] * rt_SATURATE 1874 (rtb_Switch3_m, -25.0F, 25.0F); 1875 1876 /* Stateflow: '<S16>/myMux Fun1' */ 1877 controlMCUSlugsMK_myMuxFun1(rtb_Product_c, rtb_u45, rtb_Switch3_nn, 1878 &controlMCUSlugsMKII_B.sf_myMuxFun1_o); 1879 1880 /* S-Function "dsPIC_C_function_Call" Block: <S16>/Set Log Floats 4-6 [navSupport.c] */ 1881 setLogFloat2(controlMCUSlugsMKII_B.sf_myMuxFun1_o.y); 1882 1883 /* S-Function (sfix_udelay): '<S124>/Integer Delay3' 1884 * 1885 * Regarding '<S124>/Integer Delay3': 1886 * Integer/Tapped Delay Block: '<S124>/Integer Delay3' 1887 */ 1888 rtb_IntegerDelay3_d = controlMCUSlugsMKII_DWork.IntegerDelay3_X_a; 1889 1890 /* S-Function (sfix_udelay): '<S140>/Integer Delay1' 1891 * 1892 * Regarding '<S140>/Integer Delay1': 1893 * Integer/Tapped Delay Block: '<S140>/Integer Delay1' 1894 */ 1895 rtb_IntegerDelay1_l = controlMCUSlugsMKII_DWork.IntegerDelay1_X_fi; 1896 1897 /* Switch: '<S140>/1st WP' incorporates: 1898 * Constant: '<S140>/Switch Distance WP0' 1899 * RelationalOperator: '<S177>/Compare' 1900 */ 1901 if ((rtb_IntegerDelay1_l > ((uint8_T)0U)) > 0U) { 1902 rtb_stWP = rtb_IntegerDelay3_d; 1903 } else { 1904 rtb_stWP = 100.0F; 1905 } 1906 1907 /* S-Function (sfix_udelay): '<S180>/Integer Delay1' 1908 * 1909 * Regarding '<S180>/Integer Delay1': 1910 * Integer/Tapped Delay Block: '<S180>/Integer Delay1' 1911 */ 1912 rtb_IntegerDelay3_d = controlMCUSlugsMKII_DWork.IntegerDelay1_X_i; 1913 1914 /* S-Function (sfix_udelay): '<S180>/Integer Delay2' 1915 * 1916 * Regarding '<S180>/Integer Delay2': 1917 * Integer/Tapped Delay Block: '<S180>/Integer Delay2' 1918 */ 1919 rtb_IntegerDelay2_j = controlMCUSlugsMKII_DWork.IntegerDelay2_X_e[0]; 1920 1921 /* Sum: '<S196>/Diff' incorporates: 1922 * UnitDelay: '<S196>/UD' 1923 */ 1924 controlMCUSlugsMKII_B.Diff = controlMCUSlugsMKII_B.Switch[0] - 1925 controlMCUSlugsMKII_DWork.UD_DSTATE; 1926 1927 /* S-Function "dsPIC_C_function_Call" Block: <S197>/C Function Call */ 1928 controlMCUSlugsMKII_B.CFunctionCall_j = myAbs(controlMCUSlugsMKII_B.Diff); 1929 1930 /* Saturate: '<S193>/Saturation1' */ 1931 rtb_Saturation1 = rt_SATURATE(controlMCUSlugsMKII_B.CFunctionCall_j, 0.0F, 1932 1.0F); 1933 1934 /* Sum: '<S199>/Diff' incorporates: 1935 * UnitDelay: '<S199>/UD' 1936 */ 1937 controlMCUSlugsMKII_B.Diff_o = controlMCUSlugsMKII_B.Switch[1] - 1938 controlMCUSlugsMKII_DWork.UD_DSTATE_e; 1939 1940 /* S-Function "dsPIC_C_function_Call" Block: <S200>/C Function Call */ 1941 controlMCUSlugsMKII_B.CFunctionCall_l = myAbs(controlMCUSlugsMKII_B.Diff_o); 1942 1943 /* Saturate: '<S194>/Saturation1' */ 1944 rtb_Saturation1_a = rt_SATURATE(controlMCUSlugsMKII_B.CFunctionCall_l, 0.0F, 1945 1.0F); 1946 1947 /* Sum: '<S202>/Diff' incorporates: 1948 * UnitDelay: '<S202>/UD' 1949 */ 1950 controlMCUSlugsMKII_B.Diff_a = controlMCUSlugsMKII_B.Switch[2] - 1951 controlMCUSlugsMKII_DWork.UD_DSTATE_h; 1952 1953 /* S-Function "dsPIC_C_function_Call" Block: <S203>/C Function Call */ 1954 controlMCUSlugsMKII_B.CFunctionCall_ep = myAbs(controlMCUSlugsMKII_B.Diff_a); 1955 1956 /* Sum: '<S181>/Sum' incorporates: 1957 * Saturate: '<S195>/Saturation1' 1958 */ 1959 rtb_Sum_k = (rtb_Saturation1 + rtb_Saturation1_a) + rt_SATURATE 1960 (controlMCUSlugsMKII_B.CFunctionCall_ep, 0.0F, 1.0F); 1961 1962 /* InitialCondition: '<S180>/IC' incorporates: 1963 * Saturate: '<S181>/Saturation1' 1964 */ 1965 if (controlMCUSlugsMKII_DWork.IC_FirstOutputTime_b) { 1966 controlMCUSlugsMKII_DWork.IC_FirstOutputTime_b = 0U; 1967 controlMCUSlugsMKII_B.IC_e = 1.0F; 1968 } else { 1969 controlMCUSlugsMKII_B.IC_e = rt_SATURATE(rtb_Sum_k, 0.0F, 1.0F); 1970 } 1971 1972 /* InitialCondition: '<S140>/IC' incorporates: 1973 * Sum: '<S180>/Add' 1974 */ 1975 if (controlMCUSlugsMKII_DWork.IC_FirstOutputTime_d) { 1976 controlMCUSlugsMKII_DWork.IC_FirstOutputTime_d = 0U; 1977 rtb_IC = 1.0F; 1978 } else { 1979 rtb_IC = (rtb_IntegerDelay3_d + rtb_IntegerDelay2_j) + 1980 controlMCUSlugsMKII_B.IC_e; 1981 } 1982 1983 if (rtb_IC > 0.0F) { 1984 /* Output and update for enable system: '<S140>/Compute Total Run' */ 1985 1986 /* Sum: '<S185>/Add' */ 1987 rtb_Add_d[0] = controlMCUSlugsMKII_B.Switch[0] - 1988 controlMCUSlugsMKII_B.Reshape1[0]; 1989 rtb_Add_d[1] = controlMCUSlugsMKII_B.Switch[1] - 1990 controlMCUSlugsMKII_B.Reshape1[1]; 1991 rtb_Add_d[2] = controlMCUSlugsMKII_B.Switch[2] - 1992 controlMCUSlugsMKII_B.Reshape1[2]; 1993 1994 /* Stateflow: '<S189>/Embedded MATLAB Function' */ 1995 co_EmbeddedMATLABFunction_a(rtb_Add_d, 1996 &controlMCUSlugsMKII_B.sf_EmbeddedMATLABFunction_mg); 1997 1998 /* Stateflow: '<S188>/negprotect' */ 1999 controlMCUSlug_negprotect_e 2000 (controlMCUSlugsMKII_B.sf_EmbeddedMATLABFunction_mg.xDoty_g, 2001 &controlMCUSlugsMKII_B.sf_negprotect_l); 2002 2003 /* S-Function "dsPIC_C_function_Call" Block: <S188>/C Function Call */ 2004 controlMCUSlugsMKII_B.CFunctionCall_fg = mySqrt 2005 (controlMCUSlugsMKII_B.sf_negprotect_l.zpVal_k); 2006 2007 /* Sum: '<S179>/Subtract1' */ 2008 rtb_Subtract1 = controlMCUSlugsMKII_B.CFunctionCall_fg - rtb_stWP; 2009 2010 /* Switch: '<S186>/Switch3' incorporates: 2011 * RelationalOperator: '<S186>/Relational Operator2' 2012 * S-Function (sfix_udelay): '<S186>/Integer Delay3' 2013 */ 2014 if ((rtb_Subtract1 == rtb_Subtract1) > 0U) { 2015 controlMCUSlugsMKII_B.Switch3 = rtb_Subtract1; 2016 } else { 2017 controlMCUSlugsMKII_B.Switch3 = 2018 (controlMCUSlugsMKII_DWork.IntegerDelay3_X_ft); 2019 } 2020 2021 /* Integer/Tapped Delay Block: '<S186>/Integer Delay3' 2022 */ 2023 controlMCUSlugsMKII_DWork.IntegerDelay3_X_ft = controlMCUSlugsMKII_B.Switch3; 2024 } 2025 2026 /* Sum: '<S178>/Subtract' */ 2027 rtb_Subtract_h[0] = controlMCUSlugsMKII_B.GetXYZnavSupportc[0] - 2028 controlMCUSlugsMKII_B.Switch[0]; 2029 rtb_Subtract_h[1] = controlMCUSlugsMKII_B.GetXYZnavSupportc[1] - 2030 controlMCUSlugsMKII_B.Switch[1]; 2031 rtb_Subtract_h[2] = controlMCUSlugsMKII_B.GetXYZnavSupportc[2] - 2032 controlMCUSlugsMKII_B.Switch[2]; 2033 2034 /* Stateflow: '<S183>/Embedded MATLAB Function' */ 2035 cont_EmbeddedMATLABFunction(rtb_Subtract_h, controlMCUSlugsMKII_B.Divide_n, 2036 &controlMCUSlugsMKII_B.sf_EmbeddedMATLABFunction_n); 2037 2038 /* RelationalOperator: '<S182>/Relational Operator2' */ 2039 rtb_RelationalOperator2_i = 2040 (controlMCUSlugsMKII_B.sf_EmbeddedMATLABFunction_n.xDoty == 2041 controlMCUSlugsMKII_B.sf_EmbeddedMATLABFunction_n.xDoty); 2042 2043 /* S-Function (sfix_udelay): '<S182>/Integer Delay3' 2044 * 2045 * Regarding '<S182>/Integer Delay3': 2046 * Integer/Tapped Delay Block: '<S182>/Integer Delay3' 2047 */ 2048 rtb_IntegerDelay3 = controlMCUSlugsMKII_DWork.IntegerDelay3_X; 2049 2050 /* Switch: '<S182>/Switch3' */ 2051 if (rtb_RelationalOperator2_i > 0U) { 2052 rtb_Switch3 = (real_T) 2053 controlMCUSlugsMKII_B.sf_EmbeddedMATLABFunction_n.xDoty; 2054 } else { 2055 rtb_Switch3 = rtb_IntegerDelay3; 2056 } 2057 2058 /* DataTypeConversion: '<S124>/Data Type Conversion' */ 2059 rtb_DataTypeConversion_d = (real32_T)controlMCUSlugsMKII_B.wp0; 2060 2061 /* DataTypeConversion: '<S124>/Data Type Conversion2' */ 2062 rtb_DataTypeConversion2 = (real32_T)controlMCUSlugsMKII_B.wp1; 2063 2064 /* Embedded MATLAB: '<S124>/myMux Fun2' */ 2065 2066 /* This block supports an embeddable subset of the MATLAB language. */ 2067 /* See the help menu for details. */ 2068 controlMCUSlugsMKII_B.y_e[0] = controlMCUSlugsMKII_B.Switch3; 2069 controlMCUSlugsMKII_B.y_e[1] = (real32_T)rtb_Switch3; 2070 controlMCUSlugsMKII_B.y_e[2] = rtb_DataTypeConversion_d; 2071 controlMCUSlugsMKII_B.y_e[3] = rtb_DataTypeConversion2; 2072 2073 /* S-Function "dsPIC_C_function_Call" Block: <S124>/Set The Nav Vals [navSupport.c] */ 2074 setNavNav(controlMCUSlugsMKII_B.y_e); 2075 if (rtb_LogicalOperator_e) { 2076 /* Output and update for enable system: '<S124>/Compute P' */ 2077 2078 /* Product: '<S133>/Product1' */ 2079 rtb_Product1_m = controlMCUSlugsMKII_B.navSupportc_o[1] * rtb_u45; 2080 2081 /* S-Function (sfix_udelay): '<S133>/Integer Delay1' 2082 * 2083 * Regarding '<S133>/Integer Delay1': 2084 * Integer/Tapped Delay Block: '<S133>/Integer Delay1' 2085 */ 2086 (void) memcpy(controlMCUSlugsMKII_B.IntegerDelay1, 2087 &controlMCUSlugsMKII_DWork.IntegerDelay1_X_k[0], 2088 3*sizeof(real32_T)); 2089 2090 /* S-Function "dsPIC_C_function_Call" Block: <S167>/C Function Call */ 2091 controlMCUSlugsMKII_B.CFunctionCall_l0 = myAtan2 2092 (controlMCUSlugsMKII_B.IntegerDelay1[1], 2093 controlMCUSlugsMKII_B.IntegerDelay1[0]); 2094 2095 /* Sum: '<S143>/Add' incorporates: 2096 * S-Function (sfix_udelay): '<S133>/Integer Delay2' 2097 */ 2098 rtb_Add_k[0] = (controlMCUSlugsMKII_DWork.IntegerDelay2_X_e3[0]) - 2099 controlMCUSlugsMKII_B.Reshape1[0]; 2100 rtb_Add_k[1] = (controlMCUSlugsMKII_DWork.IntegerDelay2_X_e3[1]) - 2101 controlMCUSlugsMKII_B.Reshape1[1]; 2102 rtb_Add_k[2] = (controlMCUSlugsMKII_DWork.IntegerDelay2_X_e3[2]) - 2103 controlMCUSlugsMKII_B.Reshape1[2]; 2104 2105 /* Stateflow: '<S150>/Embedded MATLAB Function' */ 2106 co_EmbeddedMATLABFunction_a(rtb_Add_k, 2107 &controlMCUSlugsMKII_B.sf_EmbeddedMATLABFunction_a); 2108 2109 /* Stateflow: '<S149>/negprotect' */ 2110 controlMCUSlug_negprotect_e 2111 (controlMCUSlugsMKII_B.sf_EmbeddedMATLABFunction_a.xDoty_g, 2112 &controlMCUSlugsMKII_B.sf_negprotect_e); 2113 2114 /* S-Function "dsPIC_C_function_Call" Block: <S149>/C Function Call */ 2115 controlMCUSlugsMKII_B.CFunctionCall_mb = mySqrt 2116 (controlMCUSlugsMKII_B.sf_negprotect_e.zpVal_k); 2117 2118 { 2119 real32_T rtmax_U0DataInY0Container; 2120 2121 /* Saturate: '<S147>/Zero Bound' */ 2122 rtmax_U0DataInY0Container = rt_MAX(controlMCUSlugsMKII_B.CFunctionCall_mb, 2123 1.000000047E-003F); 2124 2125 /* Product: '<S147>/Divide' */ 2126 controlMCUSlugsMKII_B.Divide_l[0] = rtb_Add_k[0] / 2127 rtmax_U0DataInY0Container; 2128 controlMCUSlugsMKII_B.Divide_l[1] = rtb_Add_k[1] / 2129 rtmax_U0DataInY0Container; 2130 controlMCUSlugsMKII_B.Divide_l[2] = rtb_Add_k[2] / 2131 rtmax_U0DataInY0Container; 2132 } 2133 2134 /* S-Function "dsPIC_C_function_Call" Block: <S166>/C Function Call */ 2135 controlMCUSlugsMKII_B.CFunctionCall_ir = myAtan2 2136 (controlMCUSlugsMKII_B.Divide_l[1],controlMCUSlugsMKII_B.Divide_l[0]); 2137 2138 /* Sum: '<S161>/Add' */ 2139 rtb_Add_ir = controlMCUSlugsMKII_B.CFunctionCall_l0 - 2140 controlMCUSlugsMKII_B.CFunctionCall_ir; 2141 2142 /* Embedded MATLAB: '<S161>/Embedded MATLAB Function1' */ 2143 if ((real_T)rtb_Add_ir < 0.0) { 2144 controlMCUSlugsMKII_B.angle = rtb_Add_ir + 6.283185482E+000F; 2145 } else { 2146 controlMCUSlugsMKII_B.angle = rtb_Add_ir; 2147 } 2148 2149 /* RelationalOperator: '<S145>/Larger than 180?' incorporates: 2150 * Constant: '<S145>/Constant5' 2151 */ 2152 rtb_Largerthan180 = (controlMCUSlugsMKII_B.angle >= 3.141592741E+000F); 2153 2154 /* Outputs for enable SubSystem: '<S145>/compute delta Right Turn' */ 2155 if (rtb_Largerthan180) { 2156 /* Product: '<S164>/Divide' incorporates: 2157 * Constant: '<S164>/Constant1' 2158 * Constant: '<S164>/Constant5' 2159 * Sum: '<S164>/Add' 2160 */ 2161 controlMCUSlugsMKII_B.Merge4 = (controlMCUSlugsMKII_B.angle - 2162 3.141592741E+000F) / 2.0F; 2163 } 2164 2165 /* end of Outputs for SubSystem: '<S145>/compute delta Right Turn' */ 2166 2167 /* Outputs for enable SubSystem: '<S145>/Compute delta Left Turn' incorporates: 2168 * Logic: '<S145>/Logical Operator' 2169 */ 2170 if (!rtb_Largerthan180) { 2171 /* Gain: '<S162>/Gain' incorporates: 2172 * Constant: '<S162>/Constant1' 2173 * Constant: '<S162>/Constant5' 2174 * Product: '<S162>/Divide' 2175 * Sum: '<S162>/Add' 2176 */ 2177 controlMCUSlugsMKII_B.Merge4 = (3.141592741E+000F - 2178 controlMCUSlugsMKII_B.angle) / 2.0F * -1.0F; 2179 } 2180 2181 /* end of Outputs for SubSystem: '<S145>/Compute delta Left Turn' */ 2182 2183 /* S-Function "dsPIC_C_function_Call" Block: <S163>/C Function Call */ 2184 controlMCUSlugsMKII_B.CFunctionCall_hb = mySin(controlMCUSlugsMKII_B.Merge4); 2185 2186 /* S-Function "dsPIC_C_function_Call" Block: <S163>/C Function Call1 */ 2187 controlMCUSlugsMKII_B.CFunctionCall1_i = myCos(controlMCUSlugsMKII_B.Merge4); 2188 2189 { 2190 real32_T cg_in[9]; 2191 int16_T i; 2192 real32_T rtb_Deg2R_b_idx; 2193 real32_T rtb_Deg2R_b_idx_0; 2194 2195 /* Product: '<S142>/Product1' incorporates: 2196 * Constant: '<S142>/gravity1' 2197 * Product: '<S142>/Product' 2198 */ 2199 rtb_Product1_b = controlMCUSlugsMKII_B.GetMidLevelCommandsnavSupportc[0] * 2200 controlMCUSlugsMKII_B.GetMidLevelCommandsnavSupportc[0] / 2201 9.814999580E+000F; 2202 2203 /* Embedded MATLAB: '<S145>/Compute Circles' */ 2204 { 2205 int16_T eml_i0; 2206 real_T eml_r_vec[3]; 2207 real32_T eml_absVal; 2208 2209 /* This block supports an embeddable subset of the MATLAB language. */ 2210 /* See the help menu for details. */ 2211 /* rotate the vector delta degrees */ 2212 for (eml_i0 = 0; eml_i0 < 3; eml_i0++) { 2213 eml_r_vec[eml_i0] = 0.0; 2214 } 2215 2216 eml_r_vec[0] = (real_T)((-controlMCUSlugsMKII_B.IntegerDelay1[0]) * 2217 controlMCUSlugsMKII_B.CFunctionCall1_i - 2218 controlMCUSlugsMKII_B.IntegerDelay1[1] * 2219 controlMCUSlugsMKII_B.CFunctionCall_hb); 2220 eml_r_vec[1] = (real_T)(controlMCUSlugsMKII_B.IntegerDelay1[0] * 2221 controlMCUSlugsMKII_B.CFunctionCall_hb - 2222 controlMCUSlugsMKII_B.IntegerDelay1[1] * 2223 controlMCUSlugsMKII_B.CFunctionCall1_i); 2224 if ((real_T)controlMCUSlugsMKII_B.CFunctionCall_hb < 0.0) { 2225 eml_absVal = -1.0F * controlMCUSlugsMKII_B.CFunctionCall_hb; 2226 } else { 2227 eml_absVal = controlMCUSlugsMKII_B.CFunctionCall_hb; 2228 } 2229 2230 if ((real_T)eml_absVal > 0.01) { 2231 /* compute M */ 2232 eml_absVal = rtb_Product1_b / controlMCUSlugsMKII_B.CFunctionCall_hb; 2233 if ((real_T)eml_absVal < 0.0) { 2234 eml_absVal *= -1.0F; 2235 } 2236 } else { 2237 eml_absVal = 0.0F; 2238 } 2239 2240 /* find the distance from WP */ 2241 rtb_P = eml_absVal * controlMCUSlugsMKII_B.CFunctionCall1_i; 2242 2243 /* find the center of the circle */ 2244 for (eml_i0 = 0; eml_i0 < 3; eml_i0++) { 2245 rtb_O[eml_i0] = controlMCUSlugsMKII_B.Reshape1[eml_i0] + (real32_T) 2246 eml_r_vec[eml_i0] * eml_absVal; 2247 2248 /* find the tangent points */ 2249 rtb_Pch[eml_i0] = controlMCUSlugsMKII_B.Reshape1[eml_i0] - 2250 controlMCUSlugsMKII_B.IntegerDelay1[eml_i0] * rtb_P; 2251 rtb_PchB[eml_i0] = controlMCUSlugsMKII_B.Reshape1[eml_i0] + 2252 controlMCUSlugsMKII_B.Divide_l[eml_i0] * rtb_P; 2253 } 2254 } 2255 2256 /* Sum: '<S133>/Add' */ 2257 rtb_Add_n = rtb_Product1_m + rtb_P; 2258 2259 /* Gain: '<S155>/Deg2R' */ 2260 rtb_Deg2R_b_idx = 1.745329238E-002F * 2261 controlMCUSlugsMKII_B.CFunctionCall_aj[1]; 2262 rtb_Deg2R_b_idx_0 = 1.745329238E-002F * 2263 controlMCUSlugsMKII_B.CFunctionCall_aj[0]; 2264 2265 /* SignalConversion: '<S155>/TmpHiddenBufferAtProduct1Inport1' incorporates: 2266 * Fcn: '<S158>/11' 2267 * Fcn: '<S158>/12' 2268 * Fcn: '<S158>/13' 2269 * Fcn: '<S158>/21' 2270 * Fcn: '<S158>/22' 2271 * Fcn: '<S158>/23' 2272 * Fcn: '<S158>/31' 2273 * Fcn: '<S158>/32' 2274 * Fcn: '<S158>/33' 2275 */ 2276 cg_in[0] = (real32_T)(cos(rtb_Deg2R_b_idx) * cos(rtb_Deg2R_b_idx_0)); 2277 cg_in[1] = (real32_T)(-sin(rtb_Deg2R_b_idx)); 2278 cg_in[2] = (real32_T)((-sin(rtb_Deg2R_b_idx_0)) * cos(rtb_Deg2R_b_idx)); 2279 cg_in[3] = (real32_T)(sin(rtb_Deg2R_b_idx) * cos(rtb_Deg2R_b_idx_0)); 2280 cg_in[4] = (real32_T)cos(rtb_Deg2R_b_idx); 2281 cg_in[5] = (real32_T)((-sin(rtb_Deg2R_b_idx)) * sin(rtb_Deg2R_b_idx_0)); 2282 cg_in[6] = (real32_T)sin(rtb_Deg2R_b_idx_0); 2283 cg_in[7] = 0.0F; 2284 cg_in[8] = (real32_T)cos(rtb_Deg2R_b_idx_0); 2285 for (i = 0; i < 9; i++) { 2286 rtb_TmpHiddenBufferAtProduct1_g[i] = cg_in[i]; 2287 } 2288 } 2289 2290 /* S-Function "dsPIC_C_function_Call" Block: <S146>/C Function Call */ 2291 getWP(controlMCUSlugsMKII_B.wp2,&controlMCUSlugsMKII_B.CFunctionCall_ap[0]); 2292 2293 /* Gain: '<S156>/Deg2R' */ 2294 rtb_Deg2R_a = 1.745329238E-002F * controlMCUSlugsMKII_B.CFunctionCall_ap[0]; 2295 2296 /* Trigonometry: '<S156>/sin(phi)' */ 2297 rtb_sinphi_c = (real32_T)sin(rtb_Deg2R_a); 2298 2299 /* Product: '<S156>/Rh' incorporates: 2300 * Constant: '<S156>/Re=equatorial radius' 2301 * Constant: '<S156>/const' 2302 * Fcn: '<S156>/f' 2303 * Product: '<S156>/Product1' 2304 * Product: '<S156>/sin(phi)^2' 2305 * Sum: '<S156>/Sum1' 2306 */ 2307 rtb_Rh_m = 6.378137E+006F / (real32_T)sqrt(1.0F - rtb_sinphi_c * 2308 rtb_sinphi_c * controlMCUSlugsMKII_ConstB.Sum5_c); 2309 2310 /* Sum: '<S156>/Sum2' */ 2311 rtb_Sum2_g0 = controlMCUSlugsMKII_B.CFunctionCall_ap[2] + rtb_Rh_m; 2312 2313 /* Trigonometry: '<S156>/cos(phi)' */ 2314 rtb_Deg2R_a = (real32_T)cos(rtb_Deg2R_a); 2315 2316 /* Gain: '<S156>/Deg2R1' */ 2317 rtb_Deg2R1_g = 1.745329238E-002F * controlMCUSlugsMKII_B.CFunctionCall_ap[1]; 2318 2319 /* Sum: '<S144>/Sum1' incorporates: 2320 * Product: '<S156>/(Rh+h)cos(phi)*cos(lamb)=Xe' 2321 * Product: '<S156>/(Rh+h)cos(phi)*sin(lamb)=Ye' 2322 * Product: '<S156>/Rh(1-e^2)' 2323 * Product: '<S156>/Ze' 2324 * Sum: '<S156>/Sum4' 2325 * Trigonometry: '<S156>/cos(lamb)' 2326 * Trigonometry: '<S156>/sin(lamb)' 2327 */ 2328 rtb_Sum1_g[0] = rtb_Sum2_g0 * rtb_Deg2R_a * (real32_T)cos(rtb_Deg2R1_g) - 2329 controlMCUSlugsMKII_B.RhhcosphicoslambXe; 2330 rtb_Sum1_g[1] = rtb_Sum2_g0 * rtb_Deg2R_a * (real32_T)sin(rtb_Deg2R1_g) - 2331 controlMCUSlugsMKII_B.RhhcosphisinlambYe; 2332 rtb_Sum1_g[2] = (controlMCUSlugsMKII_ConstB.e2_f * rtb_Rh_m + 2333 controlMCUSlugsMKII_B.CFunctionCall_ap[2]) * rtb_sinphi_c - 2334 controlMCUSlugsMKII_B.Ze; 2335 2336 { 2337 static const int_T dims[3] = { 3, 3, 1 }; 2338 2339 rt_MatMultRR_Sgl(rtb_Product1_d4, rtb_TmpHiddenBufferAtProduct1_g, 2340 rtb_Sum1_g, &dims[0]); 2341 } 2342 2343 /* Gain Block: '<S144>/UEN 2 NEU' 2344 * About '<S144>/UEN 2 NEU :' 2345 * Gain value: [0 0 1;0 1 0;1 0 0] 2346 2347 2348 */ 2349 { 2350 static const int_T dims[3] = { 3, 3, 1 }; 2351 2352 rt_MatMultRR_Sgl((real32_T *)rtb_UEN2NEU_l, (real32_T *) 2353 &controlMCUSlugsMKII_ConstP.pooled51[0], 2354 (real32_T *)rtb_Product1_d4, &dims[0]); 2355 } 2356 2357 /* InitialCondition: '<S133>/IC' */ 2358 if (controlMCUSlugsMKII_DWork.IC_FirstOutputTime_k) { 2359 controlMCUSlugsMKII_DWork.IC_FirstOutputTime_k = 0U; 2360 controlMCUSlugsMKII_B.IC_l = 50.0F; 2361 } else { 2362 controlMCUSlugsMKII_B.IC_l = rtb_Add_n; 2363 } 2364 2365 /* Integer/Tapped Delay Block: '<S133>/Integer Delay1' 2366 */ 2367 (void) memcpy(&controlMCUSlugsMKII_DWork.IntegerDelay1_X_k[0], 2368 controlMCUSlugsMKII_B.Divide_n, 2369 3*sizeof(real32_T)); 2370 2371 /* Integer/Tapped Delay Block: '<S133>/Integer Delay2' 2372 */ 2373 (void) memcpy(&controlMCUSlugsMKII_DWork.IntegerDelay2_X_e3[0],rtb_UEN2NEU_l, 2374 3*sizeof(real32_T)); 2375 } 2376 2377 /* RelationalOperator: '<S140>/Switch Distance Less than?' */ 2378 rtb_SwitchDistanceLessthan = (rtb_Switch3 >= (real_T) 2379 controlMCUSlugsMKII_B.Switch3); 2380 2381 /* Embedded MATLAB: '<S124>/Embedded MATLAB Function1' incorporates: 2382 * Constant: '<S124>/Switch Time Limit (in Samples)' 2383 */ 2384 { 2385 uint32_T eml_u0; 2386 uint16_T eml_u1; 2387 2388 /* Declare static variables */ 2389 rtb_swIncrement = 0.0; 2390 if ((rtb_SwitchDistanceLessthan == 1) && 2391 (controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction1.oldSw == 0) && 2392 ((real32_T) 2393 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction1.internalTimer > 2394 300.0F)) { 2395 /* if we just had a flip */ 2396 /* if enough time has passed that another switch is valid */ 2397 rtb_swIncrement = 1.0; 2398 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction1.internalTimer = 0U; 2399 } 2400 2401 eml_u0 = (uint32_T) 2402 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction1.internalTimer + 1UL; 2403 if (eml_u0 > 65535UL) { 2404 eml_u1 = MAX_uint16_T; 2405 } else { 2406 eml_u1 = (uint16_T)eml_u0; 2407 } 2408 2409 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction1.internalTimer = eml_u1; 2410 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction1.oldSw = 2411 rtb_SwitchDistanceLessthan; 2412 } 2413 2414 /* S-Function (sfix_udelay): '<S137>/Integer Delay3' 2415 * 2416 * Regarding '<S137>/Integer Delay3': 2417 * Integer/Tapped Delay Block: '<S137>/Integer Delay3' 2418 */ 2419 rtb_IntegerDelay3_d = controlMCUSlugsMKII_DWork.IntegerDelay3_X_n; 2420 2421 /* Switch: '<S137>/Switch3' incorporates: 2422 * RelationalOperator: '<S137>/Relational Operator2' 2423 */ 2424 if ((controlMCUSlugsMKII_B.IC_l == controlMCUSlugsMKII_B.IC_l) > 0U) { 2425 rtb_Switch3_e = controlMCUSlugsMKII_B.IC_l; 2426 } else { 2427 rtb_Switch3_e = rtb_IntegerDelay3_d; 2428 } 2429 2430 /* S-Function "dsPIC_C_function_Call" Block: <S126>/Set Log Floats 1-3 [navSupport.c] */ 2431 setLogFloat1(controlMCUSlugsMKII_B.L1_vec); 2432 2433 /* Product: '<S127>/Divide' incorporates: 2434 * Constant: '<S16>/Gravity' 2435 */ 2436 controlMCUSlugsMKII_B.Divide = rtb_Product_c / 9.81499958F; 2437 2438 /* S-Function (sfix_udelay): '<S271>/Integer Delay3' 2439 * 2440 * Regarding '<S271>/Integer Delay3': 2441 * Integer/Tapped Delay Block: '<S271>/Integer Delay3' 2442 */ 2443 rtb_IntegerDelay3_d = controlMCUSlugsMKII_DWork.IntegerDelay3_X_ch; 2444 2445 /* S-Function "dsPIC_C_function_Call" Block: <S272>/C Function Call */ 2446 controlMCUSlugsMKII_B.CFunctionCall_g = myAtan(controlMCUSlugsMKII_B.Divide); 2447 2448 /* Switch: '<S271>/Switch3' incorporates: 2449 * RelationalOperator: '<S271>/Relational Operator2' 2450 */ 2451 if ((controlMCUSlugsMKII_B.CFunctionCall_g == 2452 controlMCUSlugsMKII_B.CFunctionCall_g) > 0U) { 2453 rtb_Switch3_c = controlMCUSlugsMKII_B.CFunctionCall_g; 2454 } else { 2455 rtb_Switch3_c = rtb_IntegerDelay3_d; 2456 } 2457 2458 /* Integer/Tapped Delay Block: '<S261>/Integer Delay3' 2459 */ 2460 controlMCUSlugsMKII_DWork.IntegerDelay3_X_lj = rtb_Switch3_h; 2461 2462 /* Integer/Tapped Delay Block: '<S124>/Integer Delay2' 2463 */ 2464 controlMCUSlugsMKII_DWork.IntegerDelay2_X = rtb_swIncrement; 2465 2466 /* Integer/Tapped Delay Block: '<S138>/Integer Delay1' 2467 */ 2468 controlMCUSlugsMKII_DWork.IntegerDelay1_X_p = 2469 controlMCUSlugsMKII_B.HitCrossing1; 2470 2471 /* Integer/Tapped Delay Block: '<S139>/Integer Delay2' 2472 */ 2473 controlMCUSlugsMKII_DWork.IntegerDelay2_X_l = controlMCUSlugsMKII_B.IC; 2474 2475 /* Integer/Tapped Delay Block: '<S132>/Integer Delay3' 2476 */ 2477 controlMCUSlugsMKII_DWork.IntegerDelay3_X_b3 = rtb_Switch3_nn; 2478 2479 /* Integer/Tapped Delay Block: '<S254>/Integer Delay3' 2480 */ 2481 controlMCUSlugsMKII_DWork.IntegerDelay3_X_g = rtb_Switch3_a; 2482 2483 /* Integer/Tapped Delay Block: '<S268>/Integer Delay3' 2484 */ 2485 controlMCUSlugsMKII_DWork.IntegerDelay3_X_f = rtb_Switch3_m; 2486 2487 /* Integer/Tapped Delay Block: '<S124>/Integer Delay3' 2488 */ 2489 controlMCUSlugsMKII_DWork.IntegerDelay3_X_a = rtb_Switch3_e; 2490 2491 /* Integer/Tapped Delay Block: '<S140>/Integer Delay1' 2492 */ 2493 controlMCUSlugsMKII_DWork.IntegerDelay1_X_fi = controlMCUSlugsMKII_B.wp0; 2494 2495 /* Integer/Tapped Delay Block: '<S180>/Integer Delay1' 2496 */ 2497 controlMCUSlugsMKII_DWork.IntegerDelay1_X_i = controlMCUSlugsMKII_B.IC_e; 2498 2499 /* Integer/Tapped Delay Block: '<S180>/Integer Delay2' 2500 */ 2501 controlMCUSlugsMKII_DWork.IntegerDelay2_X_e[0] = 2502 controlMCUSlugsMKII_DWork.IntegerDelay2_X_e[1]; 2503 controlMCUSlugsMKII_DWork.IntegerDelay2_X_e[1] = controlMCUSlugsMKII_B.IC_e; 2504 2505 /* Update for UnitDelay: '<S196>/UD' */ 2506 controlMCUSlugsMKII_DWork.UD_DSTATE = controlMCUSlugsMKII_B.Switch[0]; 2507 2508 /* Update for UnitDelay: '<S199>/UD' */ 2509 controlMCUSlugsMKII_DWork.UD_DSTATE_e = controlMCUSlugsMKII_B.Switch[1]; 2510 2511 /* Update for UnitDelay: '<S202>/UD' */ 2512 controlMCUSlugsMKII_DWork.UD_DSTATE_h = controlMCUSlugsMKII_B.Switch[2]; 2513 2514 /* Integer/Tapped Delay Block: '<S182>/Integer Delay3' 2515 */ 2516 controlMCUSlugsMKII_DWork.IntegerDelay3_X = rtb_Switch3; 2517 2518 /* Integer/Tapped Delay Block: '<S137>/Integer Delay3' 2519 */ 2520 controlMCUSlugsMKII_DWork.IntegerDelay3_X_n = rtb_Switch3_e; 2521 2522 /* Integer/Tapped Delay Block: '<S271>/Integer Delay3' 2523 */ 2524 controlMCUSlugsMKII_DWork.IntegerDelay3_X_ch = rtb_Switch3_c; 2525 2526 /* S-Function "dsPIC_C_function_Call" Block: <Root>/[navSupport.c] */ 2527 getPidIdx(((uint8_T)8U),&controlMCUSlugsMKII_B.navSupportc_n[0]); 2528 2529 /* Output and update for atomic system: '<S2>/Longitudinal Channel' */ 2530 2531 /* Sum: '<S91>/Add' incorporates: 2532 * Constant: '<S91>/Constant from Model' 2533 * Constant: '<S91>/Constant' 2534 * Gain: '<S107>/Unit Conversion' 2535 * Product: '<S91>/Divide' 2536 * Saturate: '<S91>/[0 1000]' 2537 */ 2538 controlMCUSlugsMKII_B.Add_g = 1.0F - 3.280839920E+000F * rt_SATURATE(rtb_u020, 2539 0.0F, 1000.0F) / 145442.0F; 2540 2541 /* S-Function "dsPIC_C_function_Call" Block: <S108>/C Function Call */ 2542 controlMCUSlugsMKII_B.CFunctionCall_ab = myPow(controlMCUSlugsMKII_B.Add_g, 2543 4.255876064F); 2544 2545 /* Product: '<S91>/Divide1' incorporates: 2546 * Constant: '<S91>/Rho_0 (Kg//m^3)' 2547 */ 2548 rtb_Divide1_d = controlMCUSlugsMKII_B.CFunctionCall_ab * 1.225000024E+000F; 2549 2550 /* Product: '<S93>/Divide2' incorporates: 2551 * Constant: '<S93>/a' 2552 * Saturate: '<S93>/[ 0.01 50000]' 2553 */ 2554 rtb_Divide2 = 2.0F * controlMCUSlugsMKII_B.GetDynamicPnavSupportc / 2555 rt_SATURATE(rtb_Divide1_d, 9.999999776E-003F, 50000.0F); 2556 2557 /* Stateflow: '<S113>/negprotect' */ 2558 controlMCUSlugsM_negprotect(rtb_Divide2, 2559 &controlMCUSlugsMKII_B.sf_negprotect_c); 2560 2561 /* S-Function "dsPIC_C_function_Call" Block: <S113>/C Function Call */ 2562 controlMCUSlugsMKII_B.CFunctionCall_m4 = mySqrt 2563 (controlMCUSlugsMKII_B.sf_negprotect_c.zpVal); 2564 2565 /* RelationalOperator: '<S99>/Relational Operator2' */ 2566 rtb_RelationalOperator2_h = (controlMCUSlugsMKII_B.CFunctionCall_m4 == 2567 controlMCUSlugsMKII_B.CFunctionCall_m4); 2568 2569 /* S-Function (sfix_udelay): '<S99>/Integer Delay3' 2570 * 2571 * Regarding '<S99>/Integer Delay3': 2572 * Integer/Tapped Delay Block: '<S99>/Integer Delay3' 2573 */ 2574 rtb_Enforcelowerlimit_c = controlMCUSlugsMKII_DWork.IntegerDelay3_X_i; 2575 2576 { 2577 real32_T rateLimiterRate; 2578 2579 /* Switch: '<S99>/Switch3' */ 2580 if (rtb_RelationalOperator2_h > 0U) { 2581 rtb_Switch3_f = controlMCUSlugsMKII_B.CFunctionCall_m4; 2582 } else { 2583 rtb_Switch3_f = rtb_Enforcelowerlimit_c; 2584 } 2585 2586 /* Embedded MATLAB: '<S88>/Embedded MATLAB Function' incorporates: 2587 * Constant: '<S88>/Constant1' 2588 * Constant: '<S88>/Constant' 2589 */ 2590 { 2591 real_T eml_omega; 2592 2593 /* This block supports an embeddable subset of the MATLAB language. */ 2594 /* See the help menu for details. */ 2595 if (!controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_m.a_not_empty) { 2596 /* tf = [T*omega/(2+T*omega) T*omega/(2+T*omega)],[1 (T*omega-2)/(T*omega+2)] */ 2597 /* tf = [a a],[1 -b] */ 2598 eml_omega = 6.2831853071795862E+000 * (0.5); 2599 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_m.a = 0.01 * eml_omega / 2600 (2.0 + 0.01 * eml_omega); 2601 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_m.a_not_empty = true; 2602 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_m.b = (-(0.01 * 2603 eml_omega - 2.0)) / (0.01 * eml_omega + 2.0); 2604 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_m.y_km1 = rtb_Switch3_f; 2605 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_m.u_km1 = rtb_Switch3_f; 2606 } 2607 2608 rtb_y_i = (real32_T)controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_m.a * 2609 (rtb_Switch3_f + 2610 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_m.u_km1) + (real32_T) 2611 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_m.b * 2612 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_m.y_km1; 2613 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_m.y_km1 = rtb_y_i; 2614 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_m.u_km1 = rtb_Switch3_f; 2615 } 2616 2617 /* RelationalOperator: '<S102>/Compare' incorporates: 2618 * Constant: '<S102>/Constant' 2619 */ 2620 rtb_Compare_l = (rtb_Switch3_f <= 10.0F); 2621 2622 /* Embedded MATLAB: '<S90>/Embedded MATLAB Function' incorporates: 2623 * Constant: '<S90>/Constant1' 2624 * Constant: '<S90>/Constant' 2625 */ 2626 { 2627 real_T eml_omega; 2628 2629 /* This block supports an embeddable subset of the MATLAB language. */ 2630 /* See the help menu for details. */ 2631 if (!controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_l.a_not_empty) { 2632 /* tf = [T*omega/(2+T*omega) T*omega/(2+T*omega)],[1 (T*omega-2)/(T*omega+2)] */ 2633 /* tf = [a a],[1 -b] */ 2634 eml_omega = 6.2831853071795862E+000 * (0.032); 2635 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_l.a = 0.01 * eml_omega / 2636 (2.0 + 0.01 * eml_omega); 2637 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_l.a_not_empty = true; 2638 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_l.b = (-(0.01 * 2639 eml_omega - 2.0)) / (0.01 * eml_omega + 2.0); 2640 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_l.y_km1 = rtb_Switch3_f; 2641 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_l.u_km1 = rtb_Switch3_f; 2642 } 2643 2644 rtb_y_g = (real32_T)controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_l.a * 2645 (rtb_Switch3_f + 2646 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_l.u_km1) + (real32_T) 2647 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_l.b * 2648 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_l.y_km1; 2649 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_l.y_km1 = rtb_y_g; 2650 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_l.u_km1 = rtb_Switch3_f; 2651 } 2652 2653 /* Switch: '<S15>/Schedule LPF' */ 2654 if (rtb_Compare_l > 0U) { 2655 rtb_ScheduleLPF = rtb_y_i; 2656 } else { 2657 rtb_ScheduleLPF = rtb_y_g; 2658 } 2659 2660 /* Switch: '<S15>/Switch' incorporates: 2661 * Saturate: '<S2>/[-20 20]1' 2662 */ 2663 if (controlMCUSlugsMKII_B.wpFlynavSupportc > 0U) { 2664 rtb_Product1_ma = rt_SATURATE(controlMCUSlugsMKII_B.Reshape1[2], 170.0F, 2665 350.0F); 2666 } else { 2667 rtb_Product1_ma = controlMCUSlugsMKII_B.GetMidLevelCommandsnavSupportc[1]; 2668 } 2669 2670 /* RateLimiter: '<S15>/Rate Limiter' */ 2671 rateLimiterRate = rtb_Product1_ma - controlMCUSlugsMKII_DWork.PrevY; 2672 if (rateLimiterRate > 7.000000030E-002F) { 2673 rtb_Product1_ma = controlMCUSlugsMKII_DWork.PrevY + 7.000000030E-002F; 2674 } else if (rateLimiterRate < -5.000000075E-002F) { 2675 rtb_Product1_ma = controlMCUSlugsMKII_DWork.PrevY + -5.000000075E-002F; 2676 } 2677 2678 controlMCUSlugsMKII_DWork.PrevY = rtb_Product1_ma; 2679 2680 /* Sum: '<S15>/Add2' */ 2681 rtb_Add2 = rtb_Product1_ma - rtb_u020; 2682 } 2683 2684 /* S-Function (sfix_udelay): '<S97>/Integer Delay3' 2685 * 2686 * Regarding '<S97>/Integer Delay3': 2687 * Integer/Tapped Delay Block: '<S97>/Integer Delay3' 2688 */ 2689 rtb_IntegerDelay3_hu = controlMCUSlugsMKII_DWork.IntegerDelay3_X_m; 2690 2691 /* Switch: '<S97>/Switch3' incorporates: 2692 * RelationalOperator: '<S97>/Relational Operator2' 2693 */ 2694 if (((uint8_T)(rtb_Add2 == rtb_Add2)) > 0U) { 2695 rtb_Switch3_c1 = rtb_Add2; 2696 } else { 2697 rtb_Switch3_c1 = rtb_IntegerDelay3_hu; 2698 } 2699 2700 /* Product: '<S94>/Product1' */ 2701 rtb_Product1_ma = rtb_Switch3_c1 * 2702 controlMCUSlugsMKII_B.GetPidaccordingtoIndex1[0]; 2703 2704 /* S-Function (sfix_udelay): '<S116>/Integer Delay2' 2705 * 2706 * Regarding '<S116>/Integer Delay2': 2707 * Integer/Tapped Delay Block: '<S116>/Integer Delay2' 2708 */ 2709 rtb_IntegerDelay2_f = controlMCUSlugsMKII_DWork.IntegerDelay2_X_lq; 2710 2711 /* Sum: '<S94>/Sum2' incorporates: 2712 * Gain: '<S94>/Gain' 2713 * Memory: '<S94>/Memory1' 2714 * Product: '<S94>/Product4' 2715 */ 2716 rtb_Sum2 = (9.999999776E-003F * rtb_Switch3_c1 * 2717 controlMCUSlugsMKII_B.GetPidaccordingtoIndex1[1] + 2718 controlMCUSlugsMKII_DWork.Memory1_PreviousInput) + rtb_Product1_ma; 2719 2720 /* Switch: '<S94>/AntiWindup' incorporates: 2721 * Constant: '<S94>/Constant5' 2722 * Logic: '<S94>/Logical Operator' 2723 * RelationalOperator: '<S94>/Relational Operator' 2724 * RelationalOperator: '<S94>/Relational Operator1' 2725 */ 2726 if ((rtb_Sum2 > controlMCUSlugsMKII_ConstB.Add4) && (rtb_Sum2 < 2727 controlMCUSlugsMKII_ConstB.Add3)) { 2728 rtb_AntiWindup = (real_T)rtb_Switch3_c1; 2729 } else { 2730 rtb_AntiWindup = 0.0; 2731 } 2732 2733 /* S-Function (sfix_udelay): '<S118>/Integer Delay3' 2734 * 2735 * Regarding '<S118>/Integer Delay3': 2736 * Integer/Tapped Delay Block: '<S118>/Integer Delay3' 2737 */ 2738 rtb_IntegerDelay3_k = controlMCUSlugsMKII_DWork.IntegerDelay3_X_c; 2739 2740 /* Switch: '<S118>/Switch3' incorporates: 2741 * RelationalOperator: '<S118>/Relational Operator2' 2742 */ 2743 if (((uint8_T)(rtb_AntiWindup == rtb_AntiWindup)) > 0U) { 2744 rtb_Switch3_n = rtb_AntiWindup; 2745 } else { 2746 rtb_Switch3_n = rtb_IntegerDelay3_k; 2747 } 2748 2749 /* S-Function (sfix_udelay): '<S116>/Integer Delay' 2750 * 2751 * Regarding '<S116>/Integer Delay': 2752 * Integer/Tapped Delay Block: '<S116>/Integer Delay' 2753 */ 2754 rtb_IntegerDelay = controlMCUSlugsMKII_DWork.IntegerDelay_X; 2755 2756 /* S-Function (sfix_udelay): '<S116>/Integer Delay1' 2757 * 2758 * Regarding '<S116>/Integer Delay1': 2759 * Integer/Tapped Delay Block: '<S116>/Integer Delay1' 2760 */ 2761 rtb_IntegerDelay1 = controlMCUSlugsMKII_DWork.IntegerDelay1_X[0]; 2762 2763 /* Switch: '<S116>/Switch1' incorporates: 2764 * Constant: '<S116>/Constant' 2765 * Constant: '<S116>/Constant1' 2766 * Constant: '<S116>/Constant2' 2767 * Constant: '<S116>/Constant3' 2768 * Constant: '<S116>/Constant5' 2769 * Product: '<S116>/Product' 2770 * Product: '<S116>/Product1' 2771 * Product: '<S116>/Product2' 2772 * Product: '<S116>/Product3' 2773 * Sum: '<S116>/Subtract' 2774 * Sum: '<S116>/Subtract1' 2775 */ 2776 if (controlMCUSlugsMKII_B.ManualorAutonavSupportc > 0U) { 2777 rtb_Switch1 = 0.0F; 2778 } else { 2779 rtb_Switch1 = (((real32_T)(rtb_Switch3_n * (real_T)0.3333333433F) + 2780 (real32_T)(rtb_IntegerDelay * (real_T)1.333333373F)) + 2781 (real32_T)(rtb_IntegerDelay1 * 3.3333334326744080E-001)) * 2782 0.004999999888F + rtb_IntegerDelay2_f; 2783 } 2784 2785 /* S-Function (sfix_udelay): '<S117>/Integer Delay3' 2786 * 2787 * Regarding '<S117>/Integer Delay3': 2788 * Integer/Tapped Delay Block: '<S117>/Integer Delay3' 2789 */ 2790 rtb_IntegerDelay3_hu = controlMCUSlugsMKII_DWork.IntegerDelay3_X_at; 2791 2792 { 2793 real32_T max; 2794 2795 /* Switch: '<S117>/Switch3' incorporates: 2796 * RelationalOperator: '<S117>/Relational Operator2' 2797 */ 2798 if (((uint8_T)(rtb_Switch1 == rtb_Switch1)) > 0U) { 2799 rtb_Switch3_b = rtb_Switch1; 2800 } else { 2801 rtb_Switch3_b = rtb_IntegerDelay3_hu; 2802 } 2803 2804 /* Sum: '<S94>/Add2' incorporates: 2805 * Product: '<S94>/Product' 2806 */ 2807 rtb_Add2_i = rtb_Switch3_b * controlMCUSlugsMKII_B.GetPidaccordingtoIndex1[1] 2808 + rtb_Product1_ma; 2809 2810 /* MinMax: '<S115>/Enforce lower limit' */ 2811 max = rt_MAX(controlMCUSlugsMKII_ConstB.Add4, rtb_Add2_i); 2812 2813 /* MinMax: '<S115>/Enforce upper limit' */ 2814 max = rt_MIN(max, controlMCUSlugsMKII_ConstB.Add3); 2815 2816 /* Saturate: '<S15>/Theta_c Limit' */ 2817 rtb_Theta_cLimit = rt_SATURATE(max, -3.490658402E-001F, 3.490658402E-001F); 2818 2819 /* Embedded MATLAB: '<S15>/myMux Fun1' */ 2820 2821 /* This block supports an embeddable subset of the MATLAB language. */ 2822 /* See the help menu for details. */ 2823 controlMCUSlugsMKII_B.y_j[0] = rtb_ScheduleLPF; 2824 controlMCUSlugsMKII_B.y_j[1] = rtb_Theta_cLimit; 2825 } 2826 2827 /* S-Function "dsPIC_C_function_Call" Block: <S15>/Get XYZ [navSupport.c] */ 2828 setNavLong(controlMCUSlugsMKII_B.y_j); 2829 2830 /* RelationalOperator: '<S100>/Relational Operator2' */ 2831 rtb_RelationalOperator2_a = (controlMCUSlugsMKII_B.y_n[0] == 2832 controlMCUSlugsMKII_B.y_n[0]); 2833 2834 /* S-Function (sfix_udelay): '<S100>/Integer Delay3' 2835 * 2836 * Regarding '<S100>/Integer Delay3': 2837 * Integer/Tapped Delay Block: '<S100>/Integer Delay3' 2838 */ 2839 rtb_Add1_l = controlMCUSlugsMKII_DWork.IntegerDelay3_X_fk; 2840 2841 /* Switch: '<S100>/Switch3' */ 2842 if (rtb_RelationalOperator2_a > 0U) { 2843 rtb_Switch3_mx = controlMCUSlugsMKII_B.y_n[0]; 2844 } else { 2845 rtb_Switch3_mx = rtb_Add1_l; 2846 } 2847 2848 /* Embedded MATLAB: '<S89>/Embedded MATLAB Function' incorporates: 2849 * Constant: '<S89>/Constant1' 2850 * Constant: '<S89>/Constant' 2851 */ 2852 { 2853 real_T eml_omega; 2854 2855 /* This block supports an embeddable subset of the MATLAB language. */ 2856 /* See the help menu for details. */ 2857 if (!controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_j.a_not_empty) { 2858 /* tf = [T*omega/(2+T*omega) T*omega/(2+T*omega)],[1 (T*omega-2)/(T*omega+2)] */ 2859 /* tf = [a a],[1 -b] */ 2860 eml_omega = 6.2831853071795862E+000 * (0.32); 2861 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_j.a = 0.01 * eml_omega / 2862 (2.0 + 0.01 * eml_omega); 2863 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_j.a_not_empty = true; 2864 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_j.b = (-(0.01 * eml_omega 2865 - 2.0)) / (0.01 * eml_omega + 2.0); 2866 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_j.y_km1 = rtb_Switch3_mx; 2867 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_j.u_km1 = rtb_Switch3_mx; 2868 } 2869 2870 rtb_y_o = (real32_T)controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_j.a * 2871 (rtb_Switch3_mx + controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_j.u_km1) 2872 + (real32_T)controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_j.b * 2873 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_j.y_km1; 2874 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_j.y_km1 = rtb_y_o; 2875 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_j.u_km1 = rtb_Switch3_mx; 2876 } 2877 2878 /* Sum: '<S15>/Add' */ 2879 rtb_Add_b1 = controlMCUSlugsMKII_B.GetMidLevelCommandsnavSupportc[0] - 2880 rtb_ScheduleLPF; 2881 2882 /* Sum: '<S92>/Add4' incorporates: 2883 * Constant: '<S92>/SaturationLimit1' 2884 */ 2885 rtb_Add4 = 0.0F - controlMCUSlugsMKII_B.DataTypeConversion; 2886 2887 /* S-Function (sfix_udelay): '<S96>/Integer Delay3' 2888 * 2889 * Regarding '<S96>/Integer Delay3': 2890 * Integer/Tapped Delay Block: '<S96>/Integer Delay3' 2891 */ 2892 rtb_Add1_l = controlMCUSlugsMKII_DWork.IntegerDelay3_X_e; 2893 2894 /* Switch: '<S96>/Switch3' incorporates: 2895 * RelationalOperator: '<S96>/Relational Operator2' 2896 */ 2897 if (((uint8_T)(rtb_Add_b1 == rtb_Add_b1)) > 0U) { 2898 rtb_Switch3_kr = rtb_Add_b1; 2899 } else { 2900 rtb_Switch3_kr = rtb_Add1_l; 2901 } 2902 2903 /* Product: '<S92>/Product1' */ 2904 rtb_Product1_j = rtb_Switch3_kr * controlMCUSlugsMKII_B.GetPidaccordingtoIndex 2905 [0]; 2906 2907 /* S-Function (sfix_udelay): '<S92>/NDelays' 2908 * 2909 * Regarding '<S92>/NDelays': 2910 * Integer/Tapped Delay Block: '<S92>/NDelays' 2911 */ 2912 rtb_NDelays = controlMCUSlugsMKII_DWork.NDelays_X[0]; 2913 2914 /* Sum: '<S92>/Add1' incorporates: 2915 * Constant: '<S92>/delayTime' 2916 * Product: '<S92>/Product2' 2917 * Product: '<S92>/Product3' 2918 * Sum: '<S92>/Sum' 2919 */ 2920 rtb_Add1_l = (rtb_Switch3_kr - rtb_NDelays) / 5.000000075E-002F * 2921 controlMCUSlugsMKII_B.GetPidaccordingtoIndex[2] + rtb_Product1_j; 2922 2923 /* S-Function (sfix_udelay): '<S110>/Integer Delay2' 2924 * 2925 * Regarding '<S110>/Integer Delay2': 2926 * Integer/Tapped Delay Block: '<S110>/Integer Delay2' 2927 */ 2928 rtb_IntegerDelay2_fm = controlMCUSlugsMKII_DWork.IntegerDelay2_X_d; 2929 2930 /* Sum: '<S92>/Sum2' incorporates: 2931 * Gain: '<S92>/Gain' 2932 * Memory: '<S92>/Memory1' 2933 * Product: '<S92>/Product4' 2934 */ 2935 rtb_Sum2_k = (9.999999776E-003F * rtb_Switch3_kr * 2936 controlMCUSlugsMKII_B.GetPidaccordingtoIndex[1] + 2937 controlMCUSlugsMKII_DWork.Memory1_PreviousInput_e) + rtb_Add1_l; 2938 2939 /* Sum: '<S92>/Add3' incorporates: 2940 * Constant: '<S92>/SaturationLimit' 2941 */ 2942 rtb_Add3 = 1.0F - controlMCUSlugsMKII_B.DataTypeConversion; 2943 2944 /* Switch: '<S92>/AntiWindup' incorporates: 2945 * Constant: '<S92>/Constant5' 2946 * Logic: '<S92>/Logical Operator' 2947 * RelationalOperator: '<S92>/Relational Operator' 2948 * RelationalOperator: '<S92>/Relational Operator1' 2949 */ 2950 if ((rtb_Sum2_k > rtb_Add4) && (rtb_Sum2_k < rtb_Add3)) { 2951 rtb_AntiWindup_e = (real_T)rtb_Switch3_kr; 2952 } else { 2953 rtb_AntiWindup_e = 0.0; 2954 } 2955 2956 /* S-Function (sfix_udelay): '<S111>/Integer Delay3' 2957 * 2958 * Regarding '<S111>/Integer Delay3': 2959 * Integer/Tapped Delay Block: '<S111>/Integer Delay3' 2960 */ 2961 rtb_IntegerDelay3_k = controlMCUSlugsMKII_DWork.IntegerDelay3_X_l; 2962 2963 /* Switch: '<S111>/Switch3' incorporates: 2964 * RelationalOperator: '<S111>/Relational Operator2' 2965 */ 2966 if (((uint8_T)(rtb_AntiWindup_e == rtb_AntiWindup_e)) > 0U) { 2967 rtb_Switch3_k = rtb_AntiWindup_e; 2968 } else { 2969 rtb_Switch3_k = rtb_IntegerDelay3_k; 2970 } 2971 2972 /* S-Function (sfix_udelay): '<S110>/Integer Delay' 2973 * 2974 * Regarding '<S110>/Integer Delay': 2975 * Integer/Tapped Delay Block: '<S110>/Integer Delay' 2976 */ 2977 rtb_IntegerDelay_g = controlMCUSlugsMKII_DWork.IntegerDelay_X_i; 2978 2979 /* S-Function (sfix_udelay): '<S110>/Integer Delay1' 2980 * 2981 * Regarding '<S110>/Integer Delay1': 2982 * Integer/Tapped Delay Block: '<S110>/Integer Delay1' 2983 */ 2984 rtb_IntegerDelay1_n = controlMCUSlugsMKII_DWork.IntegerDelay1_X_o[0]; 2985 2986 /* Switch: '<S110>/Switch1' incorporates: 2987 * Constant: '<S110>/Constant' 2988 * Constant: '<S110>/Constant1' 2989 * Constant: '<S110>/Constant2' 2990 * Constant: '<S110>/Constant3' 2991 * Constant: '<S110>/Constant5' 2992 * Product: '<S110>/Product' 2993 * Product: '<S110>/Product1' 2994 * Product: '<S110>/Product2' 2995 * Product: '<S110>/Product3' 2996 * Sum: '<S110>/Subtract' 2997 * Sum: '<S110>/Subtract1' 2998 */ 2999 if (controlMCUSlugsMKII_B.ManualorAutonavSupportc > 0U) { 3000 rtb_Switch1_o = 0.0F; 3001 } else { 3002 rtb_Switch1_o = (((real32_T)(rtb_Switch3_k * (real_T)0.3333333433F) + 3003 (real32_T)(rtb_IntegerDelay_g * (real_T)1.333333373F)) + 3004 (real32_T)(rtb_IntegerDelay1_n * 3.3333334326744080E-001)) * 3005 0.004999999888F + rtb_IntegerDelay2_fm; 3006 } 3007 3008 /* S-Function (sfix_udelay): '<S112>/Integer Delay3' 3009 * 3010 * Regarding '<S112>/Integer Delay3': 3011 * Integer/Tapped Delay Block: '<S112>/Integer Delay3' 3012 */ 3013 rtb_IntegerDelay3_hu = controlMCUSlugsMKII_DWork.IntegerDelay3_X_ex; 3014 3015 { 3016 real32_T max; 3017 3018 /* Switch: '<S112>/Switch3' incorporates: 3019 * RelationalOperator: '<S112>/Relational Operator2' 3020 */ 3021 if (((uint8_T)(rtb_Switch1_o == rtb_Switch1_o)) > 0U) { 3022 rtb_Switch3_p = rtb_Switch1_o; 3023 } else { 3024 rtb_Switch3_p = rtb_IntegerDelay3_hu; 3025 } 3026 3027 /* Sum: '<S92>/Add2' incorporates: 3028 * Product: '<S92>/Product' 3029 */ 3030 rtb_Add2_d = rtb_Switch3_p * controlMCUSlugsMKII_B.GetPidaccordingtoIndex[1] 3031 + rtb_Add1_l; 3032 3033 /* MinMax: '<S109>/Enforce lower limit' */ 3034 max = rt_MAX(rtb_Add4, rtb_Add2_d); 3035 3036 /* Sum: '<S15>/Add1' incorporates: 3037 * MinMax: '<S109>/Enforce upper limit' 3038 * Product: '<S15>/Product2' 3039 */ 3040 rtb_Add1 = (rt_MIN(max, rtb_Add3) + controlMCUSlugsMKII_B.DataTypeConversion) 3041 + controlMCUSlugsMKII_B.navSupportc_n[0] * controlMCUSlugsMKII_B.y_n[1]; 3042 3043 /* Sum: '<S15>/Add3' */ 3044 rtb_Add3_i = rtb_Theta_cLimit - controlMCUSlugsMKII_B.y_n[1]; 3045 3046 /* Sum: '<S95>/Add4' incorporates: 3047 * Constant: '<S95>/SaturationLimit1' 3048 */ 3049 rtb_Add4_k = -2.617993951E-001F - controlMCUSlugsMKII_B.DataTypeConversion_c; 3050 } 3051 3052 /* S-Function (sfix_udelay): '<S98>/Integer Delay3' 3053 * 3054 * Regarding '<S98>/Integer Delay3': 3055 * Integer/Tapped Delay Block: '<S98>/Integer Delay3' 3056 */ 3057 rtb_Add1_l = controlMCUSlugsMKII_DWork.IntegerDelay3_X_nv; 3058 3059 /* Switch: '<S98>/Switch3' incorporates: 3060 * RelationalOperator: '<S98>/Relational Operator2' 3061 */ 3062 if (((uint8_T)(rtb_Add3_i == rtb_Add3_i)) > 0U) { 3063 rtb_Switch3_f3 = rtb_Add3_i; 3064 } else { 3065 rtb_Switch3_f3 = rtb_Add1_l; 3066 } 3067 3068 /* Product: '<S95>/Product1' */ 3069 rtb_Product1_a = rtb_Switch3_f3 * 3070 controlMCUSlugsMKII_B.GetPidaccordingtoIndex2[0]; 3071 3072 /* S-Function (sfix_udelay): '<S95>/NDelays' 3073 * 3074 * Regarding '<S95>/NDelays': 3075 * Integer/Tapped Delay Block: '<S95>/NDelays' 3076 */ 3077 rtb_NDelays_n = controlMCUSlugsMKII_DWork.NDelays_X_c[0]; 3078 3079 /* Sum: '<S95>/Add1' incorporates: 3080 * Constant: '<S95>/delayTime' 3081 * Product: '<S95>/Product2' 3082 * Product: '<S95>/Product3' 3083 * Sum: '<S95>/Sum' 3084 */ 3085 rtb_Add1_l = (rtb_Switch3_f3 - rtb_NDelays_n) / 5.000000075E-002F * 3086 controlMCUSlugsMKII_B.GetPidaccordingtoIndex2[2] + rtb_Product1_a; 3087 3088 /* S-Function (sfix_udelay): '<S120>/Integer Delay2' 3089 * 3090 * Regarding '<S120>/Integer Delay2': 3091 * Integer/Tapped Delay Block: '<S120>/Integer Delay2' 3092 */ 3093 rtb_IntegerDelay2_h = controlMCUSlugsMKII_DWork.IntegerDelay2_X_n; 3094 3095 /* Sum: '<S95>/Sum2' incorporates: 3096 * Gain: '<S95>/Gain' 3097 * Memory: '<S95>/Memory1' 3098 * Product: '<S95>/Product4' 3099 */ 3100 rtb_Sum2_a = (9.999999776E-003F * rtb_Switch3_f3 * 3101 controlMCUSlugsMKII_B.GetPidaccordingtoIndex2[1] + 3102 controlMCUSlugsMKII_DWork.Memory1_PreviousInput_f) + rtb_Add1_l; 3103 3104 /* Sum: '<S95>/Add3' incorporates: 3105 * Constant: '<S95>/SaturationLimit' 3106 */ 3107 rtb_Add3_b = 2.617993951E-001F - controlMCUSlugsMKII_B.DataTypeConversion_c; 3108 3109 /* Switch: '<S95>/AntiWindup' incorporates: 3110 * Constant: '<S95>/Constant5' 3111 * Logic: '<S95>/Logical Operator' 3112 * RelationalOperator: '<S95>/Relational Operator' 3113 * RelationalOperator: '<S95>/Relational Operator1' 3114 */ 3115 if ((rtb_Sum2_a > rtb_Add4_k) && (rtb_Sum2_a < rtb_Add3_b)) { 3116 rtb_AntiWindup_d = (real_T)rtb_Switch3_f3; 3117 } else { 3118 rtb_AntiWindup_d = 0.0; 3119 } 3120 3121 /* S-Function (sfix_udelay): '<S121>/Integer Delay3' 3122 * 3123 * Regarding '<S121>/Integer Delay3': 3124 * Integer/Tapped Delay Block: '<S121>/Integer Delay3' 3125 */ 3126 rtb_IntegerDelay3_k = controlMCUSlugsMKII_DWork.IntegerDelay3_X_k; 3127 3128 /* Switch: '<S121>/Switch3' incorporates: 3129 * RelationalOperator: '<S121>/Relational Operator2' 3130 */ 3131 if (((uint8_T)(rtb_AntiWindup_d == rtb_AntiWindup_d)) > 0U) { 3132 rtb_Switch3_o = rtb_AntiWindup_d; 3133 } else { 3134 rtb_Switch3_o = rtb_IntegerDelay3_k; 3135 } 3136 3137 /* S-Function (sfix_udelay): '<S120>/Integer Delay' 3138 * 3139 * Regarding '<S120>/Integer Delay': 3140 * Integer/Tapped Delay Block: '<S120>/Integer Delay' 3141 */ 3142 rtb_IntegerDelay_i = controlMCUSlugsMKII_DWork.IntegerDelay_X_g; 3143 3144 /* S-Function (sfix_udelay): '<S120>/Integer Delay1' 3145 * 3146 * Regarding '<S120>/Integer Delay1': 3147 * Integer/Tapped Delay Block: '<S120>/Integer Delay1' 3148 */ 3149 rtb_IntegerDelay1_j = controlMCUSlugsMKII_DWork.IntegerDelay1_X_oa[0]; 3150 3151 /* Switch: '<S120>/Switch1' incorporates: 3152 * Constant: '<S120>/Constant' 3153 * Constant: '<S120>/Constant1' 3154 * Constant: '<S120>/Constant2' 3155 * Constant: '<S120>/Constant3' 3156 * Constant: '<S120>/Constant5' 3157 * Product: '<S120>/Product' 3158 * Product: '<S120>/Product1' 3159 * Product: '<S120>/Product2' 3160 * Product: '<S120>/Product3' 3161 * Sum: '<S120>/Subtract' 3162 * Sum: '<S120>/Subtract1' 3163 */ 3164 if (controlMCUSlugsMKII_B.ManualorAutonavSupportc > 0U) { 3165 rtb_Switch1_k = 0.0F; 3166 } else { 3167 rtb_Switch1_k = (((real32_T)(rtb_Switch3_o * (real_T)0.3333333433F) + 3168 (real32_T)(rtb_IntegerDelay_i * (real_T)1.333333373F)) + 3169 (real32_T)(rtb_IntegerDelay1_j * 3.3333334326744080E-001)) * 3170 0.004999999888F + rtb_IntegerDelay2_h; 3171 } 3172 3173 /* S-Function (sfix_udelay): '<S122>/Integer Delay3' 3174 * 3175 * Regarding '<S122>/Integer Delay3': 3176 * Integer/Tapped Delay Block: '<S122>/Integer Delay3' 3177 */ 3178 rtb_IntegerDelay3_hu = controlMCUSlugsMKII_DWork.IntegerDelay3_X_p; 3179 3180 { 3181 real32_T max; 3182 3183 /* Switch: '<S122>/Switch3' incorporates: 3184 * RelationalOperator: '<S122>/Relational Operator2' 3185 */ 3186 if (((uint8_T)(rtb_Switch1_k == rtb_Switch1_k)) > 0U) { 3187 rtb_Switch3_el = rtb_Switch1_k; 3188 } else { 3189 rtb_Switch3_el = rtb_IntegerDelay3_hu; 3190 } 3191 3192 /* Sum: '<S95>/Add2' incorporates: 3193 * Product: '<S95>/Product' 3194 */ 3195 rtb_Add2_a = rtb_Switch3_el * controlMCUSlugsMKII_B.GetPidaccordingtoIndex2 3196 [1] + rtb_Add1_l; 3197 3198 /* MinMax: '<S119>/Enforce lower limit' */ 3199 max = rt_MAX(rtb_Add4_k, rtb_Add2_a); 3200 3201 /* MinMax: '<S119>/Enforce upper limit' */ 3202 rtb_Enforceupperlimit_m = rt_MIN(max, rtb_Add3_b); 3203 3204 /* Saturate: '<S15>/[-60 60]' */ 3205 controlMCUSlugsMKII_B.u060 = rt_SATURATE(rtb_y_o, -60.0F, 60.0F); 3206 } 3207 3208 /* S-Function "dsPIC_C_function_Call" Block: <S101>/C Function Call1 */ 3209 controlMCUSlugsMKII_B.CFunctionCall1_b = myCos(controlMCUSlugsMKII_B.u060); 3210 3211 /* Sum: '<S15>/Add4' incorporates: 3212 * Constant: '<S15>/Constant2' 3213 * Constant: '<S15>/Constant4' 3214 * Product: '<S15>/Product' 3215 * Product: '<S15>/Product1' 3216 * Sum: '<S15>/Add5' 3217 */ 3218 rtb_Add4_ki = (1.0F / controlMCUSlugsMKII_B.CFunctionCall1_b - 1.0F) * 3219 controlMCUSlugsMKII_B.GetPidaccordingtoIndex1[2] + (rtb_Enforceupperlimit_m 3220 + controlMCUSlugsMKII_B.DataTypeConversion_c); 3221 3222 /* Saturate: '<S15>/Elevator Limit' */ 3223 rtb_ElevatorLimit = rt_SATURATE(rtb_Add4_ki, -2.617993951E-001F, 3224 2.617993951E-001F); 3225 3226 /* Saturate: '<S15>/Throttle Limit' */ 3227 rtb_ThrottleLimit = rt_SATURATE(rtb_Add1, 0.0F, 1.0F); 3228 3229 /* Integer/Tapped Delay Block: '<S99>/Integer Delay3' 3230 */ 3231 controlMCUSlugsMKII_DWork.IntegerDelay3_X_i = rtb_Switch3_f; 3232 3233 /* Integer/Tapped Delay Block: '<S97>/Integer Delay3' 3234 */ 3235 controlMCUSlugsMKII_DWork.IntegerDelay3_X_m = rtb_Switch3_c1; 3236 3237 /* Integer/Tapped Delay Block: '<S116>/Integer Delay2' 3238 */ 3239 controlMCUSlugsMKII_DWork.IntegerDelay2_X_lq = rtb_Switch3_b; 3240 3241 /* Update for Memory: '<S94>/Memory1' */ 3242 controlMCUSlugsMKII_DWork.Memory1_PreviousInput = rtb_Add2_i; 3243 3244 /* Integer/Tapped Delay Block: '<S118>/Integer Delay3' 3245 */ 3246 controlMCUSlugsMKII_DWork.IntegerDelay3_X_c = rtb_Switch3_n; 3247 3248 /* Integer/Tapped Delay Block: '<S116>/Integer Delay' 3249 */ 3250 controlMCUSlugsMKII_DWork.IntegerDelay_X = rtb_Switch3_n; 3251 3252 /* Integer/Tapped Delay Block: '<S116>/Integer Delay1' 3253 */ 3254 controlMCUSlugsMKII_DWork.IntegerDelay1_X[0] = 3255 controlMCUSlugsMKII_DWork.IntegerDelay1_X[1]; 3256 controlMCUSlugsMKII_DWork.IntegerDelay1_X[1] = rtb_Switch3_n; 3257 3258 /* Integer/Tapped Delay Block: '<S117>/Integer Delay3' 3259 */ 3260 controlMCUSlugsMKII_DWork.IntegerDelay3_X_at = rtb_Switch3_b; 3261 3262 /* Integer/Tapped Delay Block: '<S100>/Integer Delay3' 3263 */ 3264 controlMCUSlugsMKII_DWork.IntegerDelay3_X_fk = rtb_Switch3_mx; 3265 3266 /* Integer/Tapped Delay Block: '<S96>/Integer Delay3' 3267 */ 3268 controlMCUSlugsMKII_DWork.IntegerDelay3_X_e = rtb_Switch3_kr; 3269 3270 /* Integer/Tapped Delay Block: '<S92>/NDelays' 3271 */ 3272 controlMCUSlugsMKII_DWork.NDelays_X[0] = controlMCUSlugsMKII_DWork.NDelays_X[1]; 3273 controlMCUSlugsMKII_DWork.NDelays_X[1] = controlMCUSlugsMKII_DWork.NDelays_X[2]; 3274 controlMCUSlugsMKII_DWork.NDelays_X[2] = controlMCUSlugsMKII_DWork.NDelays_X[3]; 3275 controlMCUSlugsMKII_DWork.NDelays_X[3] = controlMCUSlugsMKII_DWork.NDelays_X[4]; 3276 controlMCUSlugsMKII_DWork.NDelays_X[4] = rtb_Switch3_kr; 3277 3278 /* Integer/Tapped Delay Block: '<S110>/Integer Delay2' 3279 */ 3280 controlMCUSlugsMKII_DWork.IntegerDelay2_X_d = rtb_Switch3_p; 3281 3282 /* Update for Memory: '<S92>/Memory1' */ 3283 controlMCUSlugsMKII_DWork.Memory1_PreviousInput_e = rtb_Add2_d; 3284 3285 /* Integer/Tapped Delay Block: '<S111>/Integer Delay3' 3286 */ 3287 controlMCUSlugsMKII_DWork.IntegerDelay3_X_l = rtb_Switch3_k; 3288 3289 /* Integer/Tapped Delay Block: '<S110>/Integer Delay' 3290 */ 3291 controlMCUSlugsMKII_DWork.IntegerDelay_X_i = rtb_Switch3_k; 3292 3293 /* Integer/Tapped Delay Block: '<S110>/Integer Delay1' 3294 */ 3295 controlMCUSlugsMKII_DWork.IntegerDelay1_X_o[0] = 3296 controlMCUSlugsMKII_DWork.IntegerDelay1_X_o[1]; 3297 controlMCUSlugsMKII_DWork.IntegerDelay1_X_o[1] = rtb_Switch3_k; 3298 3299 /* Integer/Tapped Delay Block: '<S112>/Integer Delay3' 3300 */ 3301 controlMCUSlugsMKII_DWork.IntegerDelay3_X_ex = rtb_Switch3_p; 3302 3303 /* Integer/Tapped Delay Block: '<S98>/Integer Delay3' 3304 */ 3305 controlMCUSlugsMKII_DWork.IntegerDelay3_X_nv = rtb_Switch3_f3; 3306 3307 /* Integer/Tapped Delay Block: '<S95>/NDelays' 3308 */ 3309 controlMCUSlugsMKII_DWork.NDelays_X_c[0] = 3310 controlMCUSlugsMKII_DWork.NDelays_X_c[1]; 3311 controlMCUSlugsMKII_DWork.NDelays_X_c[1] = 3312 controlMCUSlugsMKII_DWork.NDelays_X_c[2]; 3313 controlMCUSlugsMKII_DWork.NDelays_X_c[2] = 3314 controlMCUSlugsMKII_DWork.NDelays_X_c[3]; 3315 controlMCUSlugsMKII_DWork.NDelays_X_c[3] = 3316 controlMCUSlugsMKII_DWork.NDelays_X_c[4]; 3317 controlMCUSlugsMKII_DWork.NDelays_X_c[4] = rtb_Switch3_f3; 3318 3319 /* Integer/Tapped Delay Block: '<S120>/Integer Delay2' 3320 */ 3321 controlMCUSlugsMKII_DWork.IntegerDelay2_X_n = rtb_Switch3_el; 3322 3323 /* Update for Memory: '<S95>/Memory1' */ 3324 controlMCUSlugsMKII_DWork.Memory1_PreviousInput_f = rtb_Add2_a; 3325 3326 /* Integer/Tapped Delay Block: '<S121>/Integer Delay3' 3327 */ 3328 controlMCUSlugsMKII_DWork.IntegerDelay3_X_k = rtb_Switch3_o; 3329 3330 /* Integer/Tapped Delay Block: '<S120>/Integer Delay' 3331 */ 3332 controlMCUSlugsMKII_DWork.IntegerDelay_X_g = rtb_Switch3_o; 3333 3334 /* Integer/Tapped Delay Block: '<S120>/Integer Delay1' 3335 */ 3336 controlMCUSlugsMKII_DWork.IntegerDelay1_X_oa[0] = 3337 controlMCUSlugsMKII_DWork.IntegerDelay1_X_oa[1]; 3338 controlMCUSlugsMKII_DWork.IntegerDelay1_X_oa[1] = rtb_Switch3_o; 3339 3340 /* Integer/Tapped Delay Block: '<S122>/Integer Delay3' 3341 */ 3342 controlMCUSlugsMKII_DWork.IntegerDelay3_X_p = rtb_Switch3_el; 3343 3344 /* S-Function "dsPIC_C_function_Call" Block: <S3>/Get Pid according to Index3 */ 3345 getPidIdx(((uint8_T)3U),&controlMCUSlugsMKII_B.GetPidaccordingtoIndex3[0]); 3346 3347 /* S-Function "dsPIC_C_function_Call" Block: <S3>/[navSupport.c] */ 3348 getPidIdx(((uint8_T)4U),&controlMCUSlugsMKII_B.navSupportc_k[0]); 3349 3350 /* S-Function "dsPIC_C_function_Call" Block: <S20>/[apUtils.c] */ 3351 controlMCUSlugsMKII_B.apUtilsc = myCos(controlMCUSlugsMKII_B.y_n[0]); 3352 3353 /* Product: '<S12>/Product' */ 3354 rtb_Product = controlMCUSlugsMKII_B.y_n[5] * controlMCUSlugsMKII_B.apUtilsc; 3355 3356 /* S-Function "dsPIC_C_function_Call" Block: <S21>/[apUtils.c] */ 3357 controlMCUSlugsMKII_B.apUtilsc_p = mySin(controlMCUSlugsMKII_B.y_n[0]); 3358 3359 /* Sum: '<S12>/Subtract' incorporates: 3360 * Product: '<S12>/Product1' 3361 */ 3362 rtb_Subtract = controlMCUSlugsMKII_B.y_n[4] * controlMCUSlugsMKII_B.apUtilsc_p 3363 + rtb_Product; 3364 3365 /* Switch: '<S19>/Switch3' incorporates: 3366 * RelationalOperator: '<S19>/Relational Operator2' 3367 * S-Function (sfix_udelay): '<S19>/Integer Delay3' 3368 */ 3369 if (((uint8_T)(rtb_Subtract == rtb_Subtract)) > 0U) { 3370 rtb_Switch3_oe = rtb_Subtract; 3371 } else { 3372 rtb_Switch3_oe = (controlMCUSlugsMKII_DWork.IntegerDelay3_X_b); 3373 } 3374 3375 /* S-Function "dsPIC_C_function_Call" Block: <Root>/[navSupport.c] */ 3376 getPidIdx(((uint8_T)6U),&controlMCUSlugsMKII_B.navSupportc_g[0]); 3377 if (controlMCUSlugsMKII_B.navSupportc_g[2] > 0.0F) { 3378 if (controlMCUSlugsMKII_DWork.L1OutputFeedbackControllerWithP == (int32_T) 3379 SUBSYS_DISABLED) { 3380 /* Initial conditions for enable system: '<S2>/L1 Output Feedback Controller With Projection Operator' */ 3381 3382 /* InitializeConditions for UnitDelay: '<S33>/UD' */ 3383 controlMCUSlugsMKII_DWork.UD_DSTATE_l = 0.0F; 3384 3385 /* Integer/Tapped Delay Block: '<S24>/Integer Delay' 3386 * Initialize State 3387 * Initial Condition parameter uses the same data type and scaling as Output0 3388 */ 3389 controlMCUSlugsMKII_DWork.IntegerDelay_X_d = (0.0F); 3390 3391 /* Integer/Tapped Delay Block: '<S24>/Integer Delay1' 3392 * Initialize State 3393 * Initial Condition parameter uses the same data type and scaling as Output0 3394 */ 3395 controlMCUSlugsMKII_DWork.IntegerDelay1_X_nc = (0.0F); 3396 3397 /* Integer/Tapped Delay Block: '<S26>/Integer Delay3' 3398 * Initialize State 3399 * Initial Condition parameter uses the same data type and scaling as Output0 3400 */ 3401 controlMCUSlugsMKII_DWork.IntegerDelay3_X_cw = (0.0F); 3402 3403 /* InitializeConditions for UnitDelay: '<S42>/UD' */ 3404 controlMCUSlugsMKII_DWork.UD_DSTATE_ld = 0.0F; 3405 3406 /* Integer/Tapped Delay Block: '<S28>/Integer Delay' 3407 * Initialize State 3408 * Initial Condition parameter uses the same data type and scaling as Output0 3409 */ 3410 controlMCUSlugsMKII_DWork.IntegerDelay_X_c = (0.0F); 3411 3412 /* Integer/Tapped Delay Block: '<S28>/Integer Delay1' 3413 * Initialize State 3414 * Initial Condition parameter uses the same data type and scaling as Output0 3415 */ 3416 controlMCUSlugsMKII_DWork.IntegerDelay1_X_a = (0.0F); 3417 3418 /* Integer/Tapped Delay Block: '<S13>/Integer Delay' 3419 * Initialize State 3420 * Initial Condition parameter uses the same data type and scaling as Output0 3421 */ 3422 controlMCUSlugsMKII_DWork.IntegerDelay_X_no = (0.0F); 3423 3424 /* Integer/Tapped Delay Block: '<S13>/Integer Delay1' 3425 * Initialize State 3426 * Initial Condition parameter uses the same data type and scaling as Output0 3427 */ 3428 controlMCUSlugsMKII_DWork.IntegerDelay1_X_b = (0.0F); 3429 3430 /* InitializeConditions for Merge: '<S25>/Merge' */ 3431 if (rtmIsFirstInitCond(controlMCUSlugsMKII_M)) { 3432 controlMCUSlugsMKII_B.Merge = 0.0F; 3433 } 3434 3435 /* Integer/Tapped Delay Block: '<S27>/Integer Delay3' 3436 * Initialize State 3437 * Initial Condition parameter uses the same data type and scaling as Output0 3438 */ 3439 controlMCUSlugsMKII_DWork.IntegerDelay3_X_gww = (0.0F); 3440 3441 /* Integer/Tapped Delay Block: '<S29>/Integer Delay' 3442 * Initialize State 3443 * Initial Condition parameter uses the same data type and scaling as Output0 3444 */ 3445 controlMCUSlugsMKII_DWork.IntegerDelay_X_f = (0.0F); 3446 3447 /* Integer/Tapped Delay Block: '<S29>/Integer Delay1' 3448 * Initialize State 3449 * Initial Condition parameter uses the same data type and scaling as Output0 3450 */ 3451 controlMCUSlugsMKII_DWork.IntegerDelay1_X_ny[0] = (0.0F); 3452 controlMCUSlugsMKII_DWork.IntegerDelay1_X_ny[1] = (0.0F); 3453 3454 /* Integer/Tapped Delay Block: '<S29>/Integer Delay2' 3455 * Initialize State 3456 * Initial Condition parameter uses the same data type and scaling as Output0 3457 */ 3458 controlMCUSlugsMKII_DWork.IntegerDelay2_X_m = (0.0F); 3459 3460 /* Integer/Tapped Delay Block: '<S43>/Integer Delay3' 3461 * Initialize State 3462 * Initial Condition parameter uses the same data type and scaling as Output0 3463 */ 3464 controlMCUSlugsMKII_DWork.IntegerDelay3_X_oe = (0.0F); 3465 3466 /* Integer/Tapped Delay Block: '<S44>/Integer Delay3' 3467 * Initialize State 3468 * Initial Condition parameter uses the same data type and scaling as Output0 3469 */ 3470 controlMCUSlugsMKII_DWork.IntegerDelay3_X_oe1 = (0.0F); 3471 controlMCUSlugsMKII_DWork.L1OutputFeedbackControllerWithP = (int32_T) 3472 SUBSYS_ENABLED; 3473 } 3474 } else if (controlMCUSlugsMKII_DWork.L1OutputFeedbackControllerWithP == 3475 (int32_T)SUBSYS_ENABLED) { 3476 controlMCUSlugsMKII_DWork.L1OutputFeedbackControllerWithP = (int32_T) 3477 SUBSYS_DISABLED; 3478 } 3479 3480 if (controlMCUSlugsMKII_DWork.L1OutputFeedbackControllerWithP == (int32_T) 3481 SUBSYS_ENABLED) { 3482 /* Outputs for enable system: '<S2>/L1 Output Feedback Controller With Projection Operator' */ 3483 3484 /* Sum: '<S33>/Diff' incorporates: 3485 * UnitDelay: '<S33>/UD' 3486 */ 3487 rtb_Saturation1_pt = controlMCUSlugsMKII_B.navSupportc_g[0] - 3488 controlMCUSlugsMKII_DWork.UD_DSTATE_l; 3489 3490 /* Abs: '<S31>/Abs1' */ 3491 rtb_Saturation1_pt = rt_ABS(rtb_Saturation1_pt); 3492 3493 /* InitialCondition: '<S24>/IC' incorporates: 3494 * Saturate: '<S31>/Saturation1' 3495 */ 3496 if (controlMCUSlugsMKII_DWork.IC_FirstOutputTime_g) { 3497 controlMCUSlugsMKII_DWork.IC_FirstOutputTime_g = 0U; 3498 rtb_IC_n = 1.0F; 3499 } else { 3500 rtb_IC_n = rt_SATURATE(rtb_Saturation1_pt, 0.0F, 1.0F); 3501 } 3502 3503 if (rtb_IC_n > 0.0F) { 3504 /* Output and update for enable system: '<S24>/Compute Coef' */ 3505 3506 /* Gain: '<S30>/-T' */ 3507 controlMCUSlugsMKII_B.T_i = -9.999999776E-003F * 3508 controlMCUSlugsMKII_B.navSupportc_g[0]; 3509 3510 /* S-Function "dsPIC_C_function_Call" Block: <S32>/C Function Call */ 3511 controlMCUSlugsMKII_B.CFunctionCall_p = myExp(controlMCUSlugsMKII_B.T_i); 3512 3513 /* Sum: '<S30>/1-c' incorporates: 3514 * Constant: '<S30>/Constant' 3515 */ 3516 controlMCUSlugsMKII_B.c_kt = 1.0F - controlMCUSlugsMKII_B.CFunctionCall_p; 3517 } 3518 3519 /* Product: '<S24>/Divide' incorporates: 3520 * S-Function (sfix_udelay): '<S24>/Integer Delay' 3521 */ 3522 rtb_Divide_p = controlMCUSlugsMKII_B.c_kt * 3523 (controlMCUSlugsMKII_DWork.IntegerDelay_X_d); 3524 3525 /* Sum: '<S24>/Subtract' incorporates: 3526 * Product: '<S24>/Divide1' 3527 * S-Function (sfix_udelay): '<S24>/Integer Delay1' 3528 */ 3529 rtb_Subtract_b = controlMCUSlugsMKII_B.CFunctionCall_p * 3530 (controlMCUSlugsMKII_DWork.IntegerDelay1_X_nc) + rtb_Divide_p; 3531 3532 /* Switch: '<S26>/Switch3' incorporates: 3533 * RelationalOperator: '<S26>/Relational Operator2' 3534 * S-Function (sfix_udelay): '<S26>/Integer Delay3' 3535 */ 3536 if (((uint8_T)(rtb_Switch3_oe == rtb_Switch3_oe)) > 0U) { 3537 rtb_Switch3_cj = rtb_Switch3_oe; 3538 } else { 3539 rtb_Switch3_cj = (controlMCUSlugsMKII_DWork.IntegerDelay3_X_cw); 3540 } 3541 3542 /* Sum: '<S42>/Diff' incorporates: 3543 * UnitDelay: '<S42>/UD' 3544 */ 3545 rtb_psi_ad = controlMCUSlugsMKII_B.navSupportc_g[1] - 3546 controlMCUSlugsMKII_DWork.UD_DSTATE_ld; 3547 3548 /* Abs: '<S40>/Abs1' */ 3549 rtb_psi_ad = rt_ABS(rtb_psi_ad); 3550 3551 /* InitialCondition: '<S28>/IC' incorporates: 3552 * Saturate: '<S40>/Saturation1' 3553 */ 3554 if (controlMCUSlugsMKII_DWork.IC_FirstOutputTime_p) { 3555 controlMCUSlugsMKII_DWork.IC_FirstOutputTime_p = 0U; 3556 rtb_IC_j = 1.0F; 3557 } else { 3558 rtb_IC_j = rt_SATURATE(rtb_psi_ad, 0.0F, 1.0F); 3559 } 3560 3561 if (rtb_IC_j > 0.0F) { 3562 /* Output and update for enable system: '<S28>/Compute Coef' */ 3563 3564 /* Gain: '<S39>/-T' */ 3565 controlMCUSlugsMKII_B.T = -9.999999776E-003F * 3566 controlMCUSlugsMKII_B.navSupportc_g[1]; 3567 3568 /* S-Function "dsPIC_C_function_Call" Block: <S41>/C Function Call */ 3569 controlMCUSlugsMKII_B.CFunctionCall_dr = myExp(controlMCUSlugsMKII_B.T); 3570 3571 /* Sum: '<S39>/1-c' incorporates: 3572 * Constant: '<S39>/Constant' 3573 */ 3574 controlMCUSlugsMKII_B.c_i = 1.0F - controlMCUSlugsMKII_B.CFunctionCall_dr; 3575 } 3576 3577 /* Product: '<S28>/Divide' incorporates: 3578 * S-Function (sfix_udelay): '<S28>/Integer Delay' 3579 */ 3580 rtb_Divide_f4 = controlMCUSlugsMKII_B.c_i * 3581 (controlMCUSlugsMKII_DWork.IntegerDelay_X_c); 3582 3583 /* Sum: '<S28>/Subtract' incorporates: 3584 * Product: '<S28>/Divide1' 3585 * S-Function (sfix_udelay): '<S28>/Integer Delay1' 3586 */ 3587 rtb_Subtract_c = controlMCUSlugsMKII_B.CFunctionCall_dr * 3588 (controlMCUSlugsMKII_DWork.IntegerDelay1_X_a) + rtb_Divide_f4; 3589 3590 /* Gain: '<S13>/Gain' incorporates: 3591 * Sum: '<S13>/Sum3' 3592 */ 3593 rtb_Gain_ey = (rtb_Subtract_c - rtb_Switch3_cj) * -1.0F; 3594 3595 /* Logic: '<S13>/Logical Operator' */ 3596 rtb_LogicalOperator = (uint8_T)!(controlMCUSlugsMKII_B.navSupportc_g[2] != 3597 0.0F); 3598 3599 /* Product: '<S38>/Divide4' incorporates: 3600 * Constant: '<S38>/Constant' 3601 * S-Function (sfix_udelay): '<S13>/Integer Delay1' 3602 */ 3603 rtb_Divide4_a = 1.0F / controlMCUSlugsMKII_ConstB.Divide2 * 3604 (controlMCUSlugsMKII_DWork.IntegerDelay1_X_b) * 2.0F; 3605 3606 /* Product: '<S38>/Divide3' incorporates: 3607 * Product: '<S38>/Divide' 3608 * S-Function (sfix_udelay): '<S13>/Integer Delay1' 3609 * Sum: '<S38>/Subtract' 3610 */ 3611 rtb_Divide3 = ((controlMCUSlugsMKII_DWork.IntegerDelay1_X_b) * 3612 (controlMCUSlugsMKII_DWork.IntegerDelay1_X_b) - 3613 controlMCUSlugsMKII_ConstB.Divide1) / 3614 controlMCUSlugsMKII_ConstB.Divide2; 3615 3616 /* Logic: '<S25>/Logical Operator2' incorporates: 3617 * Product: '<S25>/Divide2' 3618 * RelationalOperator: '<S34>/Compare' 3619 * RelationalOperator: '<S35>/Compare' 3620 */ 3621 rtb_LogicalOperator2 = ((rtb_Divide4_a * rtb_Gain_ey > 0.0F) && (rtb_Divide3 3622 >= 0.0F)); 3623 3624 /* Outputs for enable SubSystem: '<S25>/Enabled Subsystem' incorporates: 3625 * Logic: '<S25>/Logical Operator' 3626 */ 3627 if ((uint8_T)!(rtb_LogicalOperator2 != 0U) > 0U) { 3628 /* Inport: '<S36>/In1' */ 3629 controlMCUSlugsMKII_B.Merge = rtb_Gain_ey; 3630 } 3631 3632 /* end of Outputs for SubSystem: '<S25>/Enabled Subsystem' */ 3633 3634 /* Outputs for enable SubSystem: '<S25>/Enabled Subsystem1' */ 3635 if (rtb_LogicalOperator2 > 0U) { 3636 /* Product: '<S37>/Divide2' incorporates: 3637 * Constant: '<S37>/Constant' 3638 * Product: '<S37>/Divide1' 3639 * Signum: '<S37>/Sign' 3640 * Sum: '<S37>/Subtract' 3641 */ 3642 controlMCUSlugsMKII_B.Merge = (1.0F - rtb_Divide3 * rt_FSGN(rtb_Divide4_a)) 3643 * rtb_Gain_ey; 3644 } 3645 3646 /* end of Outputs for SubSystem: '<S25>/Enabled Subsystem1' */ 3647 3648 /* Product: '<S13>/Projection' */ 3649 rtb_Projection = controlMCUSlugsMKII_B.navSupportc_n[1] * 3650 controlMCUSlugsMKII_B.Merge; 3651 3652 /* Sum: '<S13>/Sum4' incorporates: 3653 * S-Function (sfix_udelay): '<S13>/Integer Delay' 3654 */ 3655 rtb_psi_ad = (controlMCUSlugsMKII_DWork.IntegerDelay_X_no) - rtb_Subtract_b; 3656 3657 /* Saturate: '<S13>/Psi Dot Limit' */ 3658 controlMCUSlugsMKII_B.PsiDotLimit = rt_SATURATE(rtb_psi_ad, -1.0F, 1.0F); 3659 3660 /* Sum: '<S13>/Sum2' incorporates: 3661 * S-Function (sfix_udelay): '<S13>/Integer Delay1' 3662 */ 3663 rtb_psi_ad = controlMCUSlugsMKII_B.PsiDotLimit + 3664 (controlMCUSlugsMKII_DWork.IntegerDelay1_X_b); 3665 3666 /* Switch: '<S27>/Switch3' incorporates: 3667 * RelationalOperator: '<S27>/Relational Operator2' 3668 * S-Function (sfix_udelay): '<S27>/Integer Delay3' 3669 */ 3670 if ((rtb_psi_ad == rtb_psi_ad) > 0U) { 3671 rtb_Switch3_ae = rtb_psi_ad; 3672 } else { 3673 rtb_Switch3_ae = (controlMCUSlugsMKII_DWork.IntegerDelay3_X_gww); 3674 } 3675 3676 /* S-Function (sfix_udelay): '<S29>/Integer Delay1' 3677 * 3678 * Regarding '<S29>/Integer Delay1': 3679 * Integer/Tapped Delay Block: '<S29>/Integer Delay1' 3680 */ 3681 rtb_IntegerDelay1_ov = controlMCUSlugsMKII_DWork.IntegerDelay1_X_ny[0]; 3682 3683 /* Switch: '<S43>/Switch3' incorporates: 3684 * RelationalOperator: '<S43>/Relational Operator2' 3685 * S-Function (sfix_udelay): '<S43>/Integer Delay3' 3686 */ 3687 if (((uint8_T)(rtb_Projection == rtb_Projection)) > 0U) { 3688 rtb_Switch3_gq = rtb_Projection; 3689 } else { 3690 rtb_Switch3_gq = (controlMCUSlugsMKII_DWork.IntegerDelay3_X_oe); 3691 } 3692 3693 /* Sum: '<S29>/Subtract1' incorporates: 3694 * Constant: '<S29>/Constant' 3695 * Constant: '<S29>/Constant1' 3696 * Constant: '<S29>/Constant2' 3697 * Constant: '<S29>/Constant3' 3698 * Product: '<S29>/Product' 3699 * Product: '<S29>/Product1' 3700 * Product: '<S29>/Product2' 3701 * Product: '<S29>/Product3' 3702 * S-Function (sfix_udelay): '<S29>/Integer Delay' 3703 * S-Function (sfix_udelay): '<S29>/Integer Delay2' 3704 * Sum: '<S29>/Subtract' 3705 */ 3706 rtb_Subtract1_d = (controlMCUSlugsMKII_DWork.IntegerDelay2_X_m) + 3707 (((rtb_Switch3_gq * 0.3333333433F + 3708 (controlMCUSlugsMKII_DWork.IntegerDelay_X_f) * 1.333333373F) + 3709 rtb_IntegerDelay1_ov * 0.3333333433F) * 0.004999999888F); 3710 3711 /* Switch: '<S29>/Switch1' incorporates: 3712 * Constant: '<S29>/Constant5' 3713 * Saturate: '<S29>/[min max]' 3714 */ 3715 if (rtb_LogicalOperator > 0U) { 3716 rtb_Subtract1_d = 0.0F; 3717 } else { 3718 rtb_Subtract1_d = rt_SATURATE(rtb_Subtract1_d, -2.0F, 2.0F); 3719 } 3720 3721 /* Switch: '<S44>/Switch3' incorporates: 3722 * RelationalOperator: '<S44>/Relational Operator2' 3723 * S-Function (sfix_udelay): '<S44>/Integer Delay3' 3724 */ 3725 if ((rtb_Subtract1_d == rtb_Subtract1_d) > 0U) { 3726 rtb_Switch3_im = rtb_Subtract1_d; 3727 } else { 3728 rtb_Switch3_im = (controlMCUSlugsMKII_DWork.IntegerDelay3_X_oe1); 3729 } 3730 } 3731 3732 /* S-Function "dsPIC_C_function_Call" Block: <S9>/Get the Accelerometers [navSupport.c] */ 3733 getAccels(&controlMCUSlugsMKII_B.GettheAccelerometersnavSupportc[0]); 3734 3735 { 3736 static const int_T dims[3] = { 3, 3, 1 }; 3737 3738 rt_MatMultRR_Sgl(rtb_Product2_cp, rtb_MathFunction, 3739 controlMCUSlugsMKII_B.GettheAccelerometersnavSupportc, 3740 &dims[0]); 3741 } 3742 3743 /* S-Function "dsPIC_C_function_Call" Block: <S9>/Get the Accel Bias [navSupport.c] */ 3744 getAccBias(&controlMCUSlugsMKII_B.GettheAccelBiasnavSupportc[0]); 3745 3746 { 3747 static const int_T dims[3] = { 3, 3, 1 }; 3748 3749 rt_MatMultRR_Sgl(rtb_Product3_ep, rtb_MathFunction, 3750 controlMCUSlugsMKII_B.GettheAccelBiasnavSupportc, &dims[0]); 3751 } 3752 3753 /* Sum: '<S9>/Sum' */ 3754 rtb_Sum_l[0] = rtb_Product2_cp[0] + rtb_Product3_ep[0]; 3755 rtb_Sum_l[1] = rtb_Product2_cp[1] + rtb_Product3_ep[1]; 3756 rtb_Sum_l[2] = rtb_Product2_cp[2] + rtb_Product3_ep[2]; 3757 3758 /* Output and update for atomic system: '<S2>/Lateral Channel' */ 3759 3760 /* S-Function (sfix_udelay): '<S62>/Integer Delay3' 3761 * 3762 * Regarding '<S62>/Integer Delay3': 3763 * Integer/Tapped Delay Block: '<S62>/Integer Delay3' 3764 */ 3765 rtb_IntegerDelay3_j = controlMCUSlugsMKII_DWork.IntegerDelay3_X_nc; 3766 3767 /* Switch: '<S62>/Switch3' incorporates: 3768 * RelationalOperator: '<S62>/Relational Operator2' 3769 */ 3770 if (((uint8_T)(rtb_Switch3_c == rtb_Switch3_c)) > 0U) { 3771 rtb_Switch3_gn = rtb_Switch3_c; 3772 } else { 3773 rtb_Switch3_gn = rtb_IntegerDelay3_j; 3774 } 3775 3776 /* Saturate: '<S49>/bank Limit' */ 3777 controlMCUSlugsMKII_B.bankLimit = rt_SATURATE(rtb_Switch3_gn, 3778 -7.853981853E-001F, 7.853981853E-001F); 3779 3780 /* S-Function "dsPIC_C_function_Call" Block: <S64>/C Function Call */ 3781 controlMCUSlugsMKII_B.CFunctionCall_in = myTan(controlMCUSlugsMKII_B.bankLimit); 3782 3783 /* S-Function (sfix_udelay): '<S63>/Integer Delay3' 3784 * 3785 * Regarding '<S63>/Integer Delay3': 3786 * Integer/Tapped Delay Block: '<S63>/Integer Delay3' 3787 */ 3788 rtb_IntegerDelay3_j = controlMCUSlugsMKII_DWork.IntegerDelay3_X_gw; 3789 3790 /* Switch: '<S63>/Switch3' incorporates: 3791 * RelationalOperator: '<S63>/Relational Operator2' 3792 */ 3793 if (((uint8_T)(rtb_ScheduleLPF == rtb_ScheduleLPF)) > 0U) { 3794 rtb_Switch3_h4 = rtb_ScheduleLPF; 3795 } else { 3796 rtb_Switch3_h4 = rtb_IntegerDelay3_j; 3797 } 3798 3799 /* Switch: '<S14>/Switch1' incorporates: 3800 * Constant: '<S49>/Constant1' 3801 * Product: '<S49>/Divide' 3802 * Product: '<S49>/Divide1' 3803 * Saturate: '<S49>/[0 40]' 3804 */ 3805 if (controlMCUSlugsMKII_B.wpFlynavSupportc > 0U) { 3806 rtb_IntegerDelay3_p = 1.0F / rt_SATURATE(rtb_Switch3_h4, 0.0F, 40.0F) * 3807 9.806650162E+000F * controlMCUSlugsMKII_B.CFunctionCall_in; 3808 } else { 3809 rtb_IntegerDelay3_p = controlMCUSlugsMKII_B.GetMidLevelCommandsnavSupportc[2]; 3810 } 3811 3812 /* Saturate: '<S14>/Psi Dot Limit' */ 3813 rtb_PsiDotLimit = rt_SATURATE(rtb_IntegerDelay3_p, -1.0F, 1.0F); 3814 3815 /* Switch: '<S14>/Switch' */ 3816 if (controlMCUSlugsMKII_B.navSupportc_g[2] > 3.000000119E-001F) { 3817 rtb_Switch = controlMCUSlugsMKII_B.PsiDotLimit; 3818 } else { 3819 rtb_Switch = rtb_PsiDotLimit; 3820 } 3821 3822 if (controlMCUSlugsMKII_B.navSupportc_n[2] > 0.0F) { 3823 if (controlMCUSlugsMKII_DWork.SideslipCompensation_MODE == (int32_T) 3824 SUBSYS_DISABLED) { 3825 /* Initial conditions for enable system: '<S14>/Sideslip Compensation' */ 3826 3827 /* InitializeConditions for UnitDelay: '<S79>/UD' */ 3828 controlMCUSlugsMKII_DWork.UD_DSTATE_o = 0.0F; 3829 3830 /* Integer/Tapped Delay Block: '<S71>/Integer Delay' 3831 * Initialize State 3832 * Initial Condition parameter uses the same data type and scaling as Output0 3833 */ 3834 controlMCUSlugsMKII_DWork.IntegerDelay_X_b = (0.0F); 3835 3836 /* Integer/Tapped Delay Block: '<S71>/Integer Delay1' 3837 * Initialize State 3838 * Initial Condition parameter uses the same data type and scaling as Output0 3839 */ 3840 controlMCUSlugsMKII_DWork.IntegerDelay1_X_f5 = (0.0); 3841 3842 /* Integer/Tapped Delay Block: '<S72>/Integer Delay3' 3843 * Initialize State 3844 * Initial Condition parameter uses the same data type and scaling as Output0 3845 */ 3846 controlMCUSlugsMKII_DWork.IntegerDelay3_X_hc = (0.0F); 3847 controlMCUSlugsMKII_DWork.SideslipCompensation_MODE = (int32_T) 3848 SUBSYS_ENABLED; 3849 } 3850 } else if (controlMCUSlugsMKII_DWork.SideslipCompensation_MODE == (int32_T) 3851 SUBSYS_ENABLED) { 3852 /* Disable for enable system: '<S14>/Sideslip Compensation' */ 3853 3854 /* (Virtual) Outport Block: '<S56>/bankComp' */ 3855 controlMCUSlugsMKII_B.Subtract = 0.0; 3856 controlMCUSlugsMKII_DWork.SideslipCompensation_MODE = (int32_T) 3857 SUBSYS_DISABLED; 3858 } 3859 3860 if (controlMCUSlugsMKII_DWork.SideslipCompensation_MODE == (int32_T) 3861 SUBSYS_ENABLED) { 3862 /* Output and update for enable system: '<S14>/Sideslip Compensation' */ 3863 3864 /* Stateflow: '<S70>/negprotect' */ 3865 controlMCUSlugsM_negprotect(rtb_ScheduleLPF, 3866 &controlMCUSlugsMKII_B.sf_negprotect); 3867 3868 /* Product: '<S70>/Divide' incorporates: 3869 * Constant: '<S70>/Constant1' 3870 */ 3871 rtb_Divide_l = 1.0F / controlMCUSlugsMKII_B.sf_negprotect.zpVal * 3872 9.806650162F; 3873 3874 /* Saturate: '<S70>/bank Limit' */ 3875 controlMCUSlugsMKII_B.bankLimit_a = rt_SATURATE(controlMCUSlugsMKII_B.y_n[0], 3876 -7.853981853E-001F, 7.853981853E-001F); 3877 3878 /* S-Function "dsPIC_C_function_Call" Block: <S74>/C Function Call */ 3879 controlMCUSlugsMKII_B.CFunctionCall_gd = myTan 3880 (controlMCUSlugsMKII_B.bankLimit_a); 3881 3882 /* Product: '<S70>/Divide1' */ 3883 rtb_Divide1_a = controlMCUSlugsMKII_B.CFunctionCall_gd * rtb_Divide_l; 3884 3885 /* Sum: '<S79>/Diff' incorporates: 3886 * Constant: '<S56>/Constant' 3887 * UnitDelay: '<S79>/UD' 3888 */ 3889 rtb_Diff = 3.000000119E-001F - controlMCUSlugsMKII_DWork.UD_DSTATE_o; 3890 3891 /* Abs: '<S78>/Abs1' */ 3892 rtb_Diff = rt_ABS(rtb_Diff); 3893 3894 /* InitialCondition: '<S71>/IC' incorporates: 3895 * Saturate: '<S78>/Saturation1' 3896 */ 3897 if (controlMCUSlugsMKII_DWork.IC_FirstOutputTime_e) { 3898 controlMCUSlugsMKII_DWork.IC_FirstOutputTime_e = 0U; 3899 rtb_Diff = 1.0F; 3900 } else { 3901 rtb_Diff = rt_SATURATE(rtb_Diff, 0.0F, 1.0F); 3902 } 3903 3904 /* Outputs for enable SubSystem: '<S71>/Compute Coef' */ 3905 if (rtb_Diff > 0.0F) { 3906 /* Math: '<S77>/Math Function' incorporates: 3907 * Constant: '<S56>/Constant' 3908 * Gain: '<S77>/-T' 3909 */ 3910 controlMCUSlugsMKII_B.c = (real32_T)exp(-3.000000026E-003F); 3911 3912 /* Sum: '<S77>/1-c' incorporates: 3913 * Constant: '<S77>/Constant' 3914 */ 3915 controlMCUSlugsMKII_B.c_k = 1.0F - controlMCUSlugsMKII_B.c; 3916 } 3917 3918 /* end of Outputs for SubSystem: '<S71>/Compute Coef' */ 3919 3920 /* Product: '<S71>/Divide' incorporates: 3921 * S-Function (sfix_udelay): '<S71>/Integer Delay' 3922 */ 3923 rtb_Divide_cm = controlMCUSlugsMKII_B.c_k * 3924 (controlMCUSlugsMKII_DWork.IntegerDelay_X_b); 3925 3926 /* S-Function (sfix_udelay): '<S71>/Integer Delay1' 3927 * 3928 * Regarding '<S71>/Integer Delay1': 3929 * Integer/Tapped Delay Block: '<S71>/Integer Delay1' 3930 */ 3931 rtb_IntegerDelay1_c = controlMCUSlugsMKII_DWork.IntegerDelay1_X_f5; 3932 3933 /* Sum: '<S71>/Subtract' incorporates: 3934 * Product: '<S71>/Divide1' 3935 */ 3936 controlMCUSlugsMKII_B.Subtract = (real_T)controlMCUSlugsMKII_B.c * 3937 rtb_IntegerDelay1_c + (real_T)rtb_Divide_cm; 3938 3939 /* Product: '<S73>/Divide' incorporates: 3940 * Constant: '<S73>/Constant1' 3941 * Sum: '<S56>/Subtract' 3942 */ 3943 controlMCUSlugsMKII_B.Divide_a = (rtb_Divide1_a - controlMCUSlugsMKII_B.y_n 3944 [5]) * rtb_ScheduleLPF / 9.806650162E+000F; 3945 3946 /* S-Function "dsPIC_C_function_Call" Block: <S80>/C Function Call */ 3947 controlMCUSlugsMKII_B.CFunctionCall_id = myAtan 3948 (controlMCUSlugsMKII_B.Divide_a); 3949 3950 /* Switch: '<S72>/Switch3' incorporates: 3951 * RelationalOperator: '<S72>/Relational Operator2' 3952 * S-Function (sfix_udelay): '<S72>/Integer Delay3' 3953 */ 3954 if ((controlMCUSlugsMKII_B.CFunctionCall_id == 3955 controlMCUSlugsMKII_B.CFunctionCall_id) > 0U) { 3956 rtb_Switch3_pq = controlMCUSlugsMKII_B.CFunctionCall_id; 3957 } else { 3958 rtb_Switch3_pq = (controlMCUSlugsMKII_DWork.IntegerDelay3_X_hc); 3959 } 3960 3961 /* Update for UnitDelay: '<S79>/UD' incorporates: 3962 * Constant: '<S56>/Constant' 3963 */ 3964 controlMCUSlugsMKII_DWork.UD_DSTATE_o = 3.000000119E-001F; 3965 3966 /* Integer/Tapped Delay Block: '<S71>/Integer Delay' 3967 */ 3968 controlMCUSlugsMKII_DWork.IntegerDelay_X_b = rtb_Switch3_pq; 3969 3970 /* Integer/Tapped Delay Block: '<S71>/Integer Delay1' 3971 */ 3972 controlMCUSlugsMKII_DWork.IntegerDelay1_X_f5 = 3973 controlMCUSlugsMKII_B.Subtract; 3974 3975 /* Integer/Tapped Delay Block: '<S72>/Integer Delay3' 3976 */ 3977 controlMCUSlugsMKII_DWork.IntegerDelay3_X_hc = rtb_Switch3_pq; 3978 } 3979 3980 /* Product: '<S60>/Divide' incorporates: 3981 * Constant: '<S60>/Constant1' 3982 */ 3983 controlMCUSlugsMKII_B.Divide_m = rtb_Switch * rtb_ScheduleLPF / 3984 9.806650162E+000F; 3985 3986 /* S-Function "dsPIC_C_function_Call" Block: <S86>/C Function Call */ 3987 controlMCUSlugsMKII_B.CFunctionCall_i4 = myAtan(controlMCUSlugsMKII_B.Divide_m); 3988 3989 /* Sum: '<S14>/Add2' */ 3990 rtb_IntegerDelay3_p = (real32_T)controlMCUSlugsMKII_B.Subtract + 3991 controlMCUSlugsMKII_B.CFunctionCall_i4; 3992 3993 /* Saturate: '<S14>/Bank Limit Command' */ 3994 rtb_BankLimitCommand = rt_SATURATE(rtb_IntegerDelay3_p, -7.853981853E-001F, 3995 7.853981853E-001F); 3996 3997 /* S-Function (sfix_udelay): '<S53>/Integer Delay3' 3998 * 3999 * Regarding '<S53>/Integer Delay3': 4000 * Integer/Tapped Delay Block: '<S53>/Integer Delay3' 4001 */ 4002 rtb_IntegerDelay3_p = controlMCUSlugsMKII_DWork.IntegerDelay3_X_h; 4003 4004 /* Switch: '<S53>/Switch3' incorporates: 4005 * RelationalOperator: '<S53>/Relational Operator2' 4006 */ 4007 if (((uint8_T)(rtb_Sum_l[1] == rtb_Sum_l[1])) > 0U) { 4008 rtb_Switch3_pa = rtb_Sum_l[1]; 4009 } else { 4010 rtb_Switch3_pa = rtb_IntegerDelay3_p; 4011 } 4012 4013 /* Saturate: '<S14>/[-20 20]' */ 4014 rtb_u020_g = rt_SATURATE(rtb_Switch3_pa, -20.0F, 20.0F); 4015 4016 /* Embedded MATLAB: '<S45>/Embedded MATLAB Function' incorporates: 4017 * Constant: '<S45>/Constant1' 4018 * Constant: '<S45>/Constant' 4019 */ 4020 { 4021 real_T eml_omega; 4022 4023 /* This block supports an embeddable subset of the MATLAB language. */ 4024 /* See the help menu for details. */ 4025 if (!controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction.a_not_empty) { 4026 /* tf = [T*omega/(2+T*omega) T*omega/(2+T*omega)],[1 (T*omega-2)/(T*omega+2)] */ 4027 /* tf = [a a],[1 -b] */ 4028 eml_omega = 6.2831853071795862E+000 * (0.3183); 4029 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction.a = 0.01 * eml_omega / 4030 (2.0 + 0.01 * eml_omega); 4031 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction.a_not_empty = true; 4032 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction.b = (-(0.01 * eml_omega - 4033 2.0)) / (0.01 * eml_omega + 2.0); 4034 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction.y_km1 = rtb_u020_g; 4035 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction.u_km1 = rtb_u020_g; 4036 } 4037 4038 rtb_y_m = (real32_T)controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction.a * 4039 (rtb_u020_g + controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction.u_km1) + 4040 (real32_T)controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction.b * 4041 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction.y_km1; 4042 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction.y_km1 = rtb_y_m; 4043 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction.u_km1 = rtb_u020_g; 4044 } 4045 4046 /* S-Function "dsPIC_C_function_Call" Block: <S14>/[navSupport.c] */ 4047 getPidIdx(((uint8_T)9U),&controlMCUSlugsMKII_B.navSupportc_l[0]); 4048 4049 /* RelationalOperator: '<S54>/Relational Operator2' */ 4050 rtb_RelationalOperator2_j4 = (controlMCUSlugsMKII_B.y_n[5] == 4051 controlMCUSlugsMKII_B.y_n[5]); 4052 4053 /* S-Function (sfix_udelay): '<S54>/Integer Delay3' 4054 * 4055 * Regarding '<S54>/Integer Delay3': 4056 * Integer/Tapped Delay Block: '<S54>/Integer Delay3' 4057 */ 4058 rtb_IntegerDelay3_p = controlMCUSlugsMKII_DWork.IntegerDelay3_X_ae; 4059 4060 /* Switch: '<S54>/Switch3' */ 4061 if (rtb_RelationalOperator2_j4 > 0U) { 4062 rtb_Switch3_fx = controlMCUSlugsMKII_B.y_n[5]; 4063 } else { 4064 rtb_Switch3_fx = rtb_IntegerDelay3_p; 4065 } 4066 4067 /* Saturate: '<S14>/[-2 2]' */ 4068 rtb_u2 = rt_SATURATE(rtb_Switch3_fx, -2.0F, 2.0F); 4069 4070 /* Output and update for atomic system: '<S14>/High Pass (Washout) Filter' */ 4071 4072 /* S-Function (sfix_udelay): '<S50>/Integer Delay' 4073 * 4074 * Regarding '<S50>/Integer Delay': 4075 * Integer/Tapped Delay Block: '<S50>/Integer Delay' 4076 */ 4077 rtb_IntegerDelay_k = controlMCUSlugsMKII_DWork.IntegerDelay_X_nm; 4078 4079 /* S-Function (sfix_udelay): '<S50>/Integer Delay1' 4080 * 4081 * Regarding '<S50>/Integer Delay1': 4082 * Integer/Tapped Delay Block: '<S50>/Integer Delay1' 4083 */ 4084 rtb_IntegerDelay1_a = controlMCUSlugsMKII_DWork.IntegerDelay1_X_l; 4085 4086 /* Sum: '<S50>/Subtract' incorporates: 4087 * Constant: '<S50>/Constant1' 4088 * Constant: '<S50>/Constant2' 4089 * Constant: '<S50>/Constant3' 4090 * Product: '<S50>/Product' 4091 * Product: '<S50>/Product1' 4092 * Product: '<S50>/Product2' 4093 */ 4094 rtb_Subtract_a = (0.9991999865F * rtb_u2 - rtb_IntegerDelay_k * 0.9991999865F) 4095 + rtb_IntegerDelay1_a * 0.9983000159F; 4096 4097 /* Saturate: '<S50>/[-2 2]' */ 4098 rtb_u2_o = rt_SATURATE(rtb_Subtract_a, -2.0F, 2.0F); 4099 4100 /* Integer/Tapped Delay Block: '<S50>/Integer Delay' 4101 */ 4102 controlMCUSlugsMKII_DWork.IntegerDelay_X_nm = rtb_u2; 4103 4104 /* Integer/Tapped Delay Block: '<S50>/Integer Delay1' 4105 */ 4106 controlMCUSlugsMKII_DWork.IntegerDelay1_X_l = rtb_u2_o; 4107 4108 /* Switch: '<S14>/Switch2' incorporates: 4109 * Gain: '<S14>/Neg Feedback ' 4110 * Gain: '<S14>/Neg Feedback' 4111 */ 4112 if (controlMCUSlugsMKII_B.navSupportc_l[0] > 3.000000119E-001F) { 4113 rtb_Switch2 = -1.0F * rtb_y_m; 4114 } else { 4115 rtb_Switch2 = -1.0F * rtb_u2_o; 4116 } 4117 4118 /* S-Function (sfix_udelay): '<S52>/Integer Delay3' 4119 * 4120 * Regarding '<S52>/Integer Delay3': 4121 * Integer/Tapped Delay Block: '<S52>/Integer Delay3' 4122 */ 4123 rtb_IntegerDelay3_p = controlMCUSlugsMKII_DWork.IntegerDelay3_X_b5; 4124 4125 /* Switch: '<S52>/Switch3' incorporates: 4126 * RelationalOperator: '<S52>/Relational Operator2' 4127 */ 4128 if (((uint8_T)(rtb_Switch2 == rtb_Switch2)) > 0U) { 4129 rtb_Switch3_my = rtb_Switch2; 4130 } else { 4131 rtb_Switch3_my = rtb_IntegerDelay3_p; 4132 } 4133 4134 /* Stateflow: '<S14>/myMux Fun1' */ 4135 controlMCUSlugsMK_myMuxFun1(rtb_Switch, rtb_BankLimitCommand, rtb_Switch3_my, 4136 &controlMCUSlugsMKII_B.sf_myMuxFun1_h); 4137 4138 /* S-Function "dsPIC_C_function_Call" Block: <S14>/Get XYZ [navSupport.c] */ 4139 setNavLat(controlMCUSlugsMKII_B.sf_myMuxFun1_h.y); 4140 4141 /* Sum: '<S57>/Add4' incorporates: 4142 * Constant: '<S57>/SaturationLimit1' 4143 */ 4144 rtb_Add4_m = -1.745329201E-001F - controlMCUSlugsMKII_B.DataTypeConversion_b; 4145 4146 /* Product: '<S57>/Product1' */ 4147 rtb_Product1_p = rtb_Switch3_my * 4148 controlMCUSlugsMKII_B.GetPidaccordingtoIndex3[0]; 4149 4150 /* S-Function (sfix_udelay): '<S57>/NDelays' 4151 * 4152 * Regarding '<S57>/NDelays': 4153 * Integer/Tapped Delay Block: '<S57>/NDelays' 4154 */ 4155 rtb_NDelays_no = controlMCUSlugsMKII_DWork.NDelays_X_e[0]; 4156 4157 /* Sum: '<S57>/Add1' incorporates: 4158 * Constant: '<S57>/delayTime' 4159 * Product: '<S57>/Product2' 4160 * Product: '<S57>/Product3' 4161 * Sum: '<S57>/Sum' 4162 */ 4163 rtb_IntegerDelay3_p = (rtb_Switch3_my - rtb_NDelays_no) / 5.000000075E-002F * 4164 controlMCUSlugsMKII_B.GetPidaccordingtoIndex3[2] + rtb_Product1_p; 4165 4166 /* S-Function (sfix_udelay): '<S83>/Integer Delay2' 4167 * 4168 * Regarding '<S83>/Integer Delay2': 4169 * Integer/Tapped Delay Block: '<S83>/Integer Delay2' 4170 */ 4171 rtb_IntegerDelay2_ih = controlMCUSlugsMKII_DWork.IntegerDelay2_X_g; 4172 4173 /* Sum: '<S57>/Sum2' incorporates: 4174 * Gain: '<S57>/Gain' 4175 * Memory: '<S57>/Memory1' 4176 * Product: '<S57>/Product4' 4177 */ 4178 rtb_Sum2_c = (9.999999776E-003F * rtb_Switch3_my * 4179 controlMCUSlugsMKII_B.GetPidaccordingtoIndex3[1] + 4180 controlMCUSlugsMKII_DWork.Memory1_PreviousInput_a) + 4181 rtb_IntegerDelay3_p; 4182 4183 /* Sum: '<S57>/Add3' incorporates: 4184 * Constant: '<S57>/SaturationLimit' 4185 */ 4186 rtb_Add3_k = 1.745329201E-001F - controlMCUSlugsMKII_B.DataTypeConversion_b; 4187 4188 /* Switch: '<S57>/AntiWindup' incorporates: 4189 * Constant: '<S57>/Constant5' 4190 * Logic: '<S57>/Logical Operator' 4191 * RelationalOperator: '<S57>/Relational Operator' 4192 * RelationalOperator: '<S57>/Relational Operator1' 4193 */ 4194 if ((rtb_Sum2_c > rtb_Add4_m) && (rtb_Sum2_c < rtb_Add3_k)) { 4195 rtb_AntiWindup_k = (real_T)rtb_Switch3_my; 4196 } else { 4197 rtb_AntiWindup_k = 0.0; 4198 } 4199 4200 /* S-Function (sfix_udelay): '<S84>/Integer Delay3' 4201 * 4202 * Regarding '<S84>/Integer Delay3': 4203 * Integer/Tapped Delay Block: '<S84>/Integer Delay3' 4204 */ 4205 rtb_IntegerDelay3_l = controlMCUSlugsMKII_DWork.IntegerDelay3_X_o; 4206 4207 /* Switch: '<S84>/Switch3' incorporates: 4208 * RelationalOperator: '<S84>/Relational Operator2' 4209 */ 4210 if (((uint8_T)(rtb_AntiWindup_k == rtb_AntiWindup_k)) > 0U) { 4211 rtb_Switch3_g = rtb_AntiWindup_k; 4212 } else { 4213 rtb_Switch3_g = rtb_IntegerDelay3_l; 4214 } 4215 4216 /* S-Function (sfix_udelay): '<S83>/Integer Delay' 4217 * 4218 * Regarding '<S83>/Integer Delay': 4219 * Integer/Tapped Delay Block: '<S83>/Integer Delay' 4220 */ 4221 rtb_IntegerDelay_b = controlMCUSlugsMKII_DWork.IntegerDelay_X_i0; 4222 4223 /* S-Function (sfix_udelay): '<S83>/Integer Delay1' 4224 * 4225 * Regarding '<S83>/Integer Delay1': 4226 * Integer/Tapped Delay Block: '<S83>/Integer Delay1' 4227 */ 4228 rtb_IntegerDelay1_o = controlMCUSlugsMKII_DWork.IntegerDelay1_X_f[0]; 4229 4230 /* Switch: '<S83>/Switch1' incorporates: 4231 * Constant: '<S83>/Constant' 4232 * Constant: '<S83>/Constant1' 4233 * Constant: '<S83>/Constant2' 4234 * Constant: '<S83>/Constant3' 4235 * Constant: '<S83>/Constant5' 4236 * Product: '<S83>/Product' 4237 * Product: '<S83>/Product1' 4238 * Product: '<S83>/Product2' 4239 * Product: '<S83>/Product3' 4240 * Sum: '<S83>/Subtract' 4241 * Sum: '<S83>/Subtract1' 4242 */ 4243 if (controlMCUSlugsMKII_B.ManualorAutonavSupportc > 0U) { 4244 rtb_Switch1_i = 0.0F; 4245 } else { 4246 rtb_Switch1_i = (((real32_T)(rtb_Switch3_g * (real_T)0.3333333433F) + 4247 (real32_T)(rtb_IntegerDelay_b * (real_T)1.333333373F)) + 4248 (real32_T)(rtb_IntegerDelay1_o * 3.3333334326744080E-001)) * 4249 0.004999999888F + rtb_IntegerDelay2_ih; 4250 } 4251 4252 /* RelationalOperator: '<S85>/Relational Operator2' */ 4253 rtb_RelationalOperator2_m3 = (rtb_Switch1_i == rtb_Switch1_i); 4254 4255 /* S-Function (sfix_udelay): '<S85>/Integer Delay3' 4256 * 4257 * Regarding '<S85>/Integer Delay3': 4258 * Integer/Tapped Delay Block: '<S85>/Integer Delay3' 4259 */ 4260 rtb_IntegerDelay3_j = controlMCUSlugsMKII_DWork.IntegerDelay3_X_cn; 4261 4262 { 4263 real_T tmp; 4264 4265 /* Switch: '<S85>/Switch3' incorporates: 4266 * RelationalOperator: '<S85>/Relational Operator2' 4267 */ 4268 if (rtb_RelationalOperator2_m3 > 0U) { 4269 rtb_Switch3_pf = rtb_Switch1_i; 4270 } else { 4271 rtb_Switch3_pf = rtb_IntegerDelay3_j; 4272 } 4273 4274 /* Sum: '<S57>/Add2' incorporates: 4275 * Product: '<S57>/Product' 4276 */ 4277 rtb_Add2_af = rtb_Switch3_pf * controlMCUSlugsMKII_B.GetPidaccordingtoIndex3 4278 [1] + rtb_IntegerDelay3_p; 4279 4280 /* Switch: '<S14>/Switch3' incorporates: 4281 * Gain: '<S46>/Unit Conversion' 4282 * MinMax: '<S82>/Enforce upper limit' 4283 * Sum: '<S14>/Add' 4284 */ 4285 if (controlMCUSlugsMKII_B.navSupportc_l[1] > 3.000000119E-001F) { 4286 rtb_Add1_b = 1.745329238E-002F * controlMCUSlugsMKII_B.navSupportc_l[2]; 4287 } else { 4288 /* MinMax: '<S82>/Enforce lower limit' */ 4289 rtb_Add1_b = rt_MAX(rtb_Add4_m, rtb_Add2_af); 4290 rtb_Add1_b = rt_MIN(rtb_Add1_b, rtb_Add3_k) + 4291 controlMCUSlugsMKII_B.DataTypeConversion_b; 4292 } 4293 4294 /* Saturate: '<S14>/Rudder Limit' */ 4295 rtb_RudderLimit = rt_SATURATE(rtb_Add1_b, -1.745329201E-001F, 4296 1.745329201E-001F); 4297 4298 /* Gain: '<S48>/Unit Conversion' */ 4299 rtb_UnitConversion_a = 5.729578018E+001F * rtb_RudderLimit; 4300 4301 /* Gain: '<S47>/Unit Conversion' */ 4302 rtb_UnitConversion = 5.7295779513082323E+001 * 4303 controlMCUSlugsMKII_B.Subtract; 4304 4305 /* Embedded MATLAB: '<S14>/myMux Fun3' */ 4306 4307 /* This block supports an embeddable subset of the MATLAB language. */ 4308 /* See the help menu for details. */ 4309 rtb_y_om[0] = rtb_UnitConversion_a; 4310 rtb_y_om[1] = controlMCUSlugsMKII_B.navSupportc_l[1]; 4311 rtb_y_om[2] = (real32_T)rtb_UnitConversion; 4312 4313 /* DataTypeConversion: '<S14>/Data Type Conversion2' incorporates: 4314 * Gain: '<S14>/Gain3' 4315 */ 4316 tmp = fmod(floor((real_T)(100.0F * rtb_y_om[0])), 65536.0); 4317 if (tmp < -32768.0) { 4318 tmp += 65536.0; 4319 } else if (tmp >= 32768.0) { 4320 tmp -= 65536.0; 4321 } 4322 4323 controlMCUSlugsMKII_B.DataTypeConversion2[0] = (int16_T)tmp; 4324 tmp = fmod(floor((real_T)(100.0F * rtb_y_om[1])), 65536.0); 4325 if (tmp < -32768.0) { 4326 tmp += 65536.0; 4327 } else if (tmp >= 32768.0) { 4328 tmp -= 65536.0; 4329 } 4330 4331 controlMCUSlugsMKII_B.DataTypeConversion2[1] = (int16_T)tmp; 4332 tmp = fmod(floor((real_T)(100.0F * rtb_y_om[2])), 65536.0); 4333 if (tmp < -32768.0) { 4334 tmp += 65536.0; 4335 } else if (tmp >= 32768.0) { 4336 tmp -= 65536.0; 4337 } 4338 4339 controlMCUSlugsMKII_B.DataTypeConversion2[2] = (int16_T)tmp; 4340 } 4341 4342 /* S-Function "dsPIC_C_function_Call" Block: <S14>/Get XYZ [navSupport.c]2 */ 4343 setDiagnosticShort(controlMCUSlugsMKII_B.DataTypeConversion2); 4344 4345 /* Sum: '<S14>/Add1' */ 4346 rtb_Add1_j = rtb_BankLimitCommand - controlMCUSlugsMKII_B.y_n[0]; 4347 4348 /* Sum: '<S55>/Add4' incorporates: 4349 * Constant: '<S55>/SaturationLimit1' 4350 */ 4351 rtb_Add4_p = -2.792526782E-001F - controlMCUSlugsMKII_B.DataTypeConversion_f; 4352 4353 /* S-Function (sfix_udelay): '<S51>/Integer Delay3' 4354 * 4355 * Regarding '<S51>/Integer Delay3': 4356 * Integer/Tapped Delay Block: '<S51>/Integer Delay3' 4357 */ 4358 rtb_Add1_b = controlMCUSlugsMKII_DWork.IntegerDelay3_X_mk; 4359 4360 /* Switch: '<S51>/Switch3' incorporates: 4361 * RelationalOperator: '<S51>/Relational Operator2' 4362 */ 4363 if (((uint8_T)(rtb_Add1_j == rtb_Add1_j)) > 0U) { 4364 rtb_Switch3_i = rtb_Add1_j; 4365 } else { 4366 rtb_Switch3_i = rtb_Add1_b; 4367 } 4368 4369 /* Product: '<S55>/Product1' */ 4370 rtb_Product1_mf = rtb_Switch3_i * controlMCUSlugsMKII_B.navSupportc_k[0]; 4371 4372 /* S-Function (sfix_udelay): '<S55>/NDelays' 4373 * 4374 * Regarding '<S55>/NDelays': 4375 * Integer/Tapped Delay Block: '<S55>/NDelays' 4376 */ 4377 rtb_NDelays_m = controlMCUSlugsMKII_DWork.NDelays_X_m[0]; 4378 4379 /* Sum: '<S55>/Add1' incorporates: 4380 * Constant: '<S55>/delayTime' 4381 * Product: '<S55>/Product2' 4382 * Product: '<S55>/Product3' 4383 * Sum: '<S55>/Sum' 4384 */ 4385 rtb_Add1_b = (rtb_Switch3_i - rtb_NDelays_m) / 5.000000075E-002F * 4386 controlMCUSlugsMKII_B.navSupportc_k[2] + rtb_Product1_mf; 4387 4388 /* S-Function (sfix_udelay): '<S67>/Integer Delay2' 4389 * 4390 * Regarding '<S67>/Integer Delay2': 4391 * Integer/Tapped Delay Block: '<S67>/Integer Delay2' 4392 */ 4393 rtb_IntegerDelay2_k = controlMCUSlugsMKII_DWork.IntegerDelay2_X_j; 4394 4395 /* Sum: '<S55>/Sum2' incorporates: 4396 * Gain: '<S55>/Gain' 4397 * Memory: '<S55>/Memory1' 4398 * Product: '<S55>/Product4' 4399 */ 4400 rtb_Sum2_b = (9.999999776E-003F * rtb_Switch3_i * 4401 controlMCUSlugsMKII_B.navSupportc_k[1] + 4402 controlMCUSlugsMKII_DWork.Memory1_PreviousInput_l) + rtb_Add1_b; 4403 4404 /* Sum: '<S55>/Add3' incorporates: 4405 * Constant: '<S55>/SaturationLimit' 4406 */ 4407 rtb_Add3_o = 2.792526782E-001F - controlMCUSlugsMKII_B.DataTypeConversion_f; 4408 4409 /* Switch: '<S55>/AntiWindup' incorporates: 4410 * Constant: '<S55>/Constant5' 4411 * Logic: '<S55>/Logical Operator' 4412 * RelationalOperator: '<S55>/Relational Operator' 4413 * RelationalOperator: '<S55>/Relational Operator1' 4414 */ 4415 if ((rtb_Sum2_b > rtb_Add4_p) && (rtb_Sum2_b < rtb_Add3_o)) { 4416 rtb_AntiWindup_m = (real_T)rtb_Switch3_i; 4417 } else { 4418 rtb_AntiWindup_m = 0.0; 4419 } 4420 4421 /* S-Function (sfix_udelay): '<S68>/Integer Delay3' 4422 * 4423 * Regarding '<S68>/Integer Delay3': 4424 * Integer/Tapped Delay Block: '<S68>/Integer Delay3' 4425 */ 4426 rtb_IntegerDelay3_l = controlMCUSlugsMKII_DWork.IntegerDelay3_X_j; 4427 4428 /* Switch: '<S68>/Switch3' incorporates: 4429 * RelationalOperator: '<S68>/Relational Operator2' 4430 */ 4431 if (((uint8_T)(rtb_AntiWindup_m == rtb_AntiWindup_m)) > 0U) { 4432 rtb_Switch3_na = rtb_AntiWindup_m; 4433 } else { 4434 rtb_Switch3_na = rtb_IntegerDelay3_l; 4435 } 4436 4437 /* S-Function (sfix_udelay): '<S67>/Integer Delay' 4438 * 4439 * Regarding '<S67>/Integer Delay': 4440 * Integer/Tapped Delay Block: '<S67>/Integer Delay' 4441 */ 4442 rtb_IntegerDelay_c = controlMCUSlugsMKII_DWork.IntegerDelay_X_n; 4443 4444 /* S-Function (sfix_udelay): '<S67>/Integer Delay1' 4445 * 4446 * Regarding '<S67>/Integer Delay1': 4447 * Integer/Tapped Delay Block: '<S67>/Integer Delay1' 4448 */ 4449 rtb_IntegerDelay1_d = controlMCUSlugsMKII_DWork.IntegerDelay1_X_n[0]; 4450 4451 /* Switch: '<S67>/Switch1' incorporates: 4452 * Constant: '<S67>/Constant' 4453 * Constant: '<S67>/Constant1' 4454 * Constant: '<S67>/Constant2' 4455 * Constant: '<S67>/Constant3' 4456 * Constant: '<S67>/Constant5' 4457 * Product: '<S67>/Product' 4458 * Product: '<S67>/Product1' 4459 * Product: '<S67>/Product2' 4460 * Product: '<S67>/Product3' 4461 * Sum: '<S67>/Subtract' 4462 * Sum: '<S67>/Subtract1' 4463 */ 4464 if (controlMCUSlugsMKII_B.ManualorAutonavSupportc > 0U) { 4465 rtb_Switch1_g = 0.0F; 4466 } else { 4467 rtb_Switch1_g = (((real32_T)(rtb_Switch3_na * (real_T)0.3333333433F) + 4468 (real32_T)(rtb_IntegerDelay_c * (real_T)1.333333373F)) + 4469 (real32_T)(rtb_IntegerDelay1_d * 3.3333334326744080E-001)) * 4470 0.004999999888F + rtb_IntegerDelay2_k; 4471 } 4472 4473 /* S-Function (sfix_udelay): '<S69>/Integer Delay3' 4474 * 4475 * Regarding '<S69>/Integer Delay3': 4476 * Integer/Tapped Delay Block: '<S69>/Integer Delay3' 4477 */ 4478 rtb_IntegerDelay3_p = controlMCUSlugsMKII_DWork.IntegerDelay3_X_hp; 4479 4480 { 4481 real32_T max; 4482 4483 /* Switch: '<S69>/Switch3' incorporates: 4484 * RelationalOperator: '<S69>/Relational Operator2' 4485 */ 4486 if (((uint8_T)(rtb_Switch1_g == rtb_Switch1_g)) > 0U) { 4487 rtb_Switch3_cv = rtb_Switch1_g; 4488 } else { 4489 rtb_Switch3_cv = rtb_IntegerDelay3_p; 4490 } 4491 4492 /* Sum: '<S55>/Add2' incorporates: 4493 * Product: '<S55>/Product' 4494 */ 4495 rtb_Add2_n = rtb_Switch3_cv * controlMCUSlugsMKII_B.navSupportc_k[1] + 4496 rtb_Add1_b; 4497 4498 /* MinMax: '<S66>/Enforce lower limit' */ 4499 max = rt_MAX(rtb_Add4_p, rtb_Add2_n); 4500 4501 /* Sum: '<S14>/Add3' incorporates: 4502 * MinMax: '<S66>/Enforce upper limit' 4503 */ 4504 max = rt_MIN(max, rtb_Add3_o) + controlMCUSlugsMKII_B.DataTypeConversion_f; 4505 4506 /* Saturate: '<S14>/Aileron Limit' */ 4507 rtb_AileronLimit = rt_SATURATE(max, -2.792526782E-001F, 2.792526782E-001F); 4508 } 4509 4510 /* Integer/Tapped Delay Block: '<S62>/Integer Delay3' 4511 */ 4512 controlMCUSlugsMKII_DWork.IntegerDelay3_X_nc = rtb_Switch3_gn; 4513 4514 /* Integer/Tapped Delay Block: '<S63>/Integer Delay3' 4515 */ 4516 controlMCUSlugsMKII_DWork.IntegerDelay3_X_gw = rtb_Switch3_h4; 4517 4518 /* Integer/Tapped Delay Block: '<S53>/Integer Delay3' 4519 */ 4520 controlMCUSlugsMKII_DWork.IntegerDelay3_X_h = rtb_Switch3_pa; 4521 4522 /* Integer/Tapped Delay Block: '<S54>/Integer Delay3' 4523 */ 4524 controlMCUSlugsMKII_DWork.IntegerDelay3_X_ae = rtb_Switch3_fx; 4525 4526 /* Integer/Tapped Delay Block: '<S52>/Integer Delay3' 4527 */ 4528 controlMCUSlugsMKII_DWork.IntegerDelay3_X_b5 = rtb_Switch3_my; 4529 4530 /* Integer/Tapped Delay Block: '<S57>/NDelays' 4531 */ 4532 controlMCUSlugsMKII_DWork.NDelays_X_e[0] = 4533 controlMCUSlugsMKII_DWork.NDelays_X_e[1]; 4534 controlMCUSlugsMKII_DWork.NDelays_X_e[1] = 4535 controlMCUSlugsMKII_DWork.NDelays_X_e[2]; 4536 controlMCUSlugsMKII_DWork.NDelays_X_e[2] = 4537 controlMCUSlugsMKII_DWork.NDelays_X_e[3]; 4538 controlMCUSlugsMKII_DWork.NDelays_X_e[3] = 4539 controlMCUSlugsMKII_DWork.NDelays_X_e[4]; 4540 controlMCUSlugsMKII_DWork.NDelays_X_e[4] = rtb_Switch3_my; 4541 4542 /* Integer/Tapped Delay Block: '<S83>/Integer Delay2' 4543 */ 4544 controlMCUSlugsMKII_DWork.IntegerDelay2_X_g = rtb_Switch3_pf; 4545 4546 /* Update for Memory: '<S57>/Memory1' */ 4547 controlMCUSlugsMKII_DWork.Memory1_PreviousInput_a = rtb_Add2_af; 4548 4549 /* Integer/Tapped Delay Block: '<S84>/Integer Delay3' 4550 */ 4551 controlMCUSlugsMKII_DWork.IntegerDelay3_X_o = rtb_Switch3_g; 4552 4553 /* Integer/Tapped Delay Block: '<S83>/Integer Delay' 4554 */ 4555 controlMCUSlugsMKII_DWork.IntegerDelay_X_i0 = rtb_Switch3_g; 4556 4557 /* Integer/Tapped Delay Block: '<S83>/Integer Delay1' 4558 */ 4559 controlMCUSlugsMKII_DWork.IntegerDelay1_X_f[0] = 4560 controlMCUSlugsMKII_DWork.IntegerDelay1_X_f[1]; 4561 controlMCUSlugsMKII_DWork.IntegerDelay1_X_f[1] = rtb_Switch3_g; 4562 4563 /* Integer/Tapped Delay Block: '<S85>/Integer Delay3' 4564 */ 4565 controlMCUSlugsMKII_DWork.IntegerDelay3_X_cn = rtb_Switch3_pf; 4566 4567 /* Integer/Tapped Delay Block: '<S51>/Integer Delay3' 4568 */ 4569 controlMCUSlugsMKII_DWork.IntegerDelay3_X_mk = rtb_Switch3_i; 4570 4571 /* Integer/Tapped Delay Block: '<S55>/NDelays' 4572 */ 4573 controlMCUSlugsMKII_DWork.NDelays_X_m[0] = 4574 controlMCUSlugsMKII_DWork.NDelays_X_m[1]; 4575 controlMCUSlugsMKII_DWork.NDelays_X_m[1] = 4576 controlMCUSlugsMKII_DWork.NDelays_X_m[2]; 4577 controlMCUSlugsMKII_DWork.NDelays_X_m[2] = 4578 controlMCUSlugsMKII_DWork.NDelays_X_m[3]; 4579 controlMCUSlugsMKII_DWork.NDelays_X_m[3] = 4580 controlMCUSlugsMKII_DWork.NDelays_X_m[4]; 4581 controlMCUSlugsMKII_DWork.NDelays_X_m[4] = rtb_Switch3_i; 4582 4583 /* Integer/Tapped Delay Block: '<S67>/Integer Delay2' 4584 */ 4585 controlMCUSlugsMKII_DWork.IntegerDelay2_X_j = rtb_Switch3_cv; 4586 4587 /* Update for Memory: '<S55>/Memory1' */ 4588 controlMCUSlugsMKII_DWork.Memory1_PreviousInput_l = rtb_Add2_n; 4589 4590 /* Integer/Tapped Delay Block: '<S68>/Integer Delay3' 4591 */ 4592 controlMCUSlugsMKII_DWork.IntegerDelay3_X_j = rtb_Switch3_na; 4593 4594 /* Integer/Tapped Delay Block: '<S67>/Integer Delay' 4595 */ 4596 controlMCUSlugsMKII_DWork.IntegerDelay_X_n = rtb_Switch3_na; 4597 4598 /* Integer/Tapped Delay Block: '<S67>/Integer Delay1' 4599 */ 4600 controlMCUSlugsMKII_DWork.IntegerDelay1_X_n[0] = 4601 controlMCUSlugsMKII_DWork.IntegerDelay1_X_n[1]; 4602 controlMCUSlugsMKII_DWork.IntegerDelay1_X_n[1] = rtb_Switch3_na; 4603 4604 /* Integer/Tapped Delay Block: '<S69>/Integer Delay3' 4605 */ 4606 controlMCUSlugsMKII_DWork.IntegerDelay3_X_hp = rtb_Switch3_cv; 4607 4608 /* Embedded MATLAB: '<Root>/myMux Fun1' */ 4609 4610 /* This block supports an embeddable subset of the MATLAB language. */ 4611 /* See the help menu for details. */ 4612 rtb_y[0] = rtb_ThrottleLimit; 4613 rtb_y[1] = rtb_AileronLimit; 4614 rtb_y[2] = rtb_RudderLimit; 4615 rtb_y[3] = rtb_ElevatorLimit; 4616 4617 /* If: '<S291>/If' */ 4618 if (controlMCUSlugsMKII_B.GetControlTypenavSupportc == 1U) { 4619 /* Output and update for action system: '<S291>/If Control Type Is Manual' */ 4620 4621 /* Gain: '<S293>/Gain' */ 4622 rtb_Gain_fi = rtb_y_n[0] >> 1; 4623 4624 /* Gain: '<S293>/Gain1' */ 4625 rtb_Gain1_cp = rtb_y_n[1] >> 1; 4626 4627 /* Gain: '<S293>/Gain2' */ 4628 rtb_Gain2_fp = rtb_y_n[3] >> 1; 4629 4630 /* Gain: '<S293>/Gain3' */ 4631 rtb_Gain3_j = rtb_y_n[2] >> 1; 4632 4633 /* Stateflow: '<S293>/myMux Fun1' */ 4634 controlMCUSlugs_myMuxFun1_a(rtb_Gain_fi, rtb_Gain1_cp, rtb_Gain2_fp, 4635 rtb_Gain3_j, controlMCUSlugsMKII_B.Merge_n); 4636 } else if ((controlMCUSlugsMKII_B.GetControlTypenavSupportc == 2U) || 4637 (controlMCUSlugsMKII_B.GetControlTypenavSupportc == 3U)) { 4638 /* Output and update for action system: '<S291>/If Control Type Is Wp Follow or Inner Cmds' */ 4639 { 4640 real_T tmp; 4641 4642 /* DataTypeConversion: '<S304>/Data Type Conversion' incorporates: 4643 * Constant: '<S304>/Constant1' 4644 * Constant: '<S304>/Constant2' 4645 * Product: '<S304>/Divide' 4646 * Sum: '<S304>/Add' 4647 */ 4648 tmp = fmod(floor((real_T)rtb_y[3] * 3.9411311193641618E+003 + 4649 3.7764285714285725E+003), 65536.0); 4650 if (tmp < 0.0) { 4651 tmp += 65536.0; 4652 } 4653 4654 rtb_DataTypeConversion_k = (uint16_T)tmp; 4655 4656 /* DataTypeConversion: '<S305>/Data Type Conversion' incorporates: 4657 * Constant: '<S305>/Constant1' 4658 * Constant: '<S305>/Constant2' 4659 * Product: '<S305>/Divide' 4660 * Sum: '<S305>/Add' 4661 */ 4662 tmp = fmod(floor((real_T)rtb_y[0] * 1274.0 + 3176.0), 65536.0); 4663 if (tmp < 0.0) { 4664 tmp += 65536.0; 4665 } 4666 4667 rtb_DataTypeConversion_o = (uint16_T)tmp; 4668 4669 /* DataTypeConversion: '<S306>/Data Type Conversion' incorporates: 4670 * Constant: '<S306>/Constant1' 4671 * Constant: '<S306>/Constant2' 4672 * Product: '<S306>/Divide' 4673 * Sum: '<S306>/Add' 4674 */ 4675 tmp = fmod(floor((real_T)rtb_y[1] * 3.4254691037464213E+003 + 4676 3.6921428571428564E+003), 65536.0); 4677 if (tmp < 0.0) { 4678 tmp += 65536.0; 4679 } 4680 4681 rtb_DataTypeConversion_o5 = (uint16_T)tmp; 4682 4683 /* DataTypeConversion: '<S307>/Data Type Conversion' incorporates: 4684 * Constant: '<S307>/Constant1' 4685 * Constant: '<S307>/Constant2' 4686 * Product: '<S307>/Divide' 4687 * Sum: '<S307>/Add' 4688 */ 4689 tmp = fmod(floor((real_T)rtb_y[2] * 2.8579680495216048E+003 + 4690 3.7214285714285706E+003), 65536.0); 4691 if (tmp < 0.0) { 4692 tmp += 65536.0; 4693 } 4694 4695 rtb_DataTypeConversion_j = (uint16_T)tmp; 4696 } 4697 4698 /* Stateflow: '<S296>/myMux Fun1' */ 4699 controlMCUSlugs_myMuxFun1_a(rtb_DataTypeConversion_o, 4700 rtb_DataTypeConversion_o5, rtb_DataTypeConversion_j, 4701 rtb_DataTypeConversion_k, controlMCUSlugsMKII_B.Merge_n); 4702 } else if (controlMCUSlugsMKII_B.GetControlTypenavSupportc == 4U) { 4703 /* Output and update for action system: '<S291>/If Control Type Is Passthrough' */ 4704 4705 /* Gain: '<S294>/Gain' */ 4706 rtb_Gain_pn = rtb_y_n[0] >> 1; 4707 4708 /* Gain: '<S294>/Gain1' */ 4709 rtb_Gain1_c = rtb_y_n[1] >> 1; 4710 4711 /* Gain: '<S294>/Gain2' */ 4712 rtb_Gain2_d = rtb_y_n[3] >> 1; 4713 4714 /* Gain: '<S294>/Gain3' */ 4715 rtb_Gain3_p = rtb_y_n[2] >> 1; 4716 4717 /* Stateflow: '<S294>/myMux Fun1' */ 4718 controlMCUSlugs_myMuxFun1_a(rtb_Gain_pn, rtb_Gain1_c, rtb_Gain2_d, 4719 rtb_Gain3_p, controlMCUSlugsMKII_B.Merge_n); 4720 } else if (controlMCUSlugsMKII_B.GetControlTypenavSupportc == 5U) { 4721 /* Output and update for action system: '<S291>/If Control Type Is Selective Passthrough' */ 4722 4723 /* S-Function "dsPIC_C_function_Call" Block: <S295>/Get Pass Switches [navSupport.c] */ 4724 getPassValues(&controlMCUSlugsMKII_B.GetPassSwitchesnavSupportc[0]); 4725 4726 { 4727 real_T tmp; 4728 4729 /* Switch: '<S295>/Switch' incorporates: 4730 * Constant: '<S300>/Constant1' 4731 * Constant: '<S300>/Constant2' 4732 * DataTypeConversion: '<S300>/Data Type Conversion' 4733 * Gain: '<S295>/Gain' 4734 * Product: '<S300>/Divide' 4735 * Sum: '<S300>/Add' 4736 */ 4737 if (controlMCUSlugsMKII_B.GetPassSwitchesnavSupportc[3] >= 1U) { 4738 rtb_Switch_k = rtb_y_n[0] >> 1; 4739 } else { 4740 tmp = fmod(floor((real_T)rtb_y[0] * 1274.0 + 3176.0), 65536.0); 4741 if (tmp < 0.0) { 4742 tmp += 65536.0; 4743 } 4744 4745 rtb_Switch_k = (uint16_T)tmp; 4746 } 4747 4748 /* Switch: '<S295>/Switch1' incorporates: 4749 * Constant: '<S301>/Constant1' 4750 * Constant: '<S301>/Constant2' 4751 * DataTypeConversion: '<S301>/Data Type Conversion' 4752 * Gain: '<S295>/Gain1' 4753 * Product: '<S301>/Divide' 4754 * Sum: '<S301>/Add' 4755 */ 4756 if (controlMCUSlugsMKII_B.GetPassSwitchesnavSupportc[2] >= 1U) { 4757 rtb_Switch1_bs = rtb_y_n[1] >> 1; 4758 } else { 4759 tmp = fmod(floor((real_T)rtb_y[1] * 3.4254691037464213E+003 + 4760 3.6921428571428564E+003), 65536.0); 4761 if (tmp < 0.0) { 4762 tmp += 65536.0; 4763 } 4764 4765 rtb_Switch1_bs = (uint16_T)tmp; 4766 } 4767 4768 /* Switch: '<S295>/Switch2' incorporates: 4769 * Constant: '<S302>/Constant1' 4770 * Constant: '<S302>/Constant2' 4771 * DataTypeConversion: '<S302>/Data Type Conversion' 4772 * Gain: '<S295>/Gain2' 4773 * Product: '<S302>/Divide' 4774 * Sum: '<S302>/Add' 4775 */ 4776 if (controlMCUSlugsMKII_B.GetPassSwitchesnavSupportc[1] >= 1U) { 4777 rtb_Switch2_l = rtb_y_n[3] >> 1; 4778 } else { 4779 tmp = fmod(floor((real_T)rtb_y[2] * 2.8579680495216048E+003 + 4780 3.7214285714285706E+003), 65536.0); 4781 if (tmp < 0.0) { 4782 tmp += 65536.0; 4783 } 4784 4785 rtb_Switch2_l = (uint16_T)tmp; 4786 } 4787 4788 /* Switch: '<S295>/Switch3' incorporates: 4789 * Constant: '<S299>/Constant1' 4790 * Constant: '<S299>/Constant2' 4791 * DataTypeConversion: '<S299>/Data Type Conversion' 4792 * Gain: '<S295>/Gain3' 4793 * Product: '<S299>/Divide' 4794 * Sum: '<S299>/Add' 4795 */ 4796 if (controlMCUSlugsMKII_B.GetPassSwitchesnavSupportc[0] >= 1U) { 4797 rtb_Switch3_c5 = rtb_y_n[2] >> 1; 4798 } else { 4799 tmp = fmod(floor((real_T)rtb_y[3] * 3.9411311193641618E+003 + 4800 3.7764285714285725E+003), 65536.0); 4801 if (tmp < 0.0) { 4802 tmp += 65536.0; 4803 } 4804 4805 rtb_Switch3_c5 = (uint16_T)tmp; 4806 } 4807 } 4808 4809 /* Stateflow: '<S295>/myMux Fun1' */ 4810 controlMCUSlugs_myMuxFun1_a(rtb_Switch_k, rtb_Switch1_bs, rtb_Switch2_l, 4811 rtb_Switch3_c5, controlMCUSlugsMKII_B.Merge_n); 4812 } 4813 4814 /* S-Function "dsPIC_PWM_motor" Block: <S8>/PWM Servo Output */ 4815 PDC1 = controlMCUSlugsMKII_B.Merge_n[2] ; 4816 PDC2 = controlMCUSlugsMKII_B.Merge_n[3] ; 4817 PDC3 = controlMCUSlugsMKII_B.Merge_n[1] ; 4818 PDC4 = controlMCUSlugsMKII_B.Merge_n[0] ; 4819 4820 /* SignalConversion: '<S292>/TmpHiddenBufferAt SFunction Inport1' */ 4821 rtb_TmpHiddenBufferAtSFunctionI[0] = controlMCUSlugsMKII_B.Merge_n[0]; 4822 rtb_TmpHiddenBufferAtSFunctionI[1] = controlMCUSlugsMKII_B.Merge_n[1]; 4823 rtb_TmpHiddenBufferAtSFunctionI[2] = controlMCUSlugsMKII_B.Merge_n[3]; 4824 rtb_TmpHiddenBufferAtSFunctionI[3] = controlMCUSlugsMKII_B.Merge_n[2]; 4825 4826 /* Embedded MATLAB: '<S8>/myMux Fun1' */ 4827 { 4828 int16_T eml_i0; 4829 4830 /* This block supports an embeddable subset of the MATLAB language. */ 4831 /* See the help menu for details. */ 4832 for (eml_i0 = 0; eml_i0 < 4; eml_i0++) { 4833 controlMCUSlugsMKII_B.y_d[eml_i0] = rtb_TmpHiddenBufferAtSFunctionI[eml_i0]; 4834 } 4835 4836 controlMCUSlugsMKII_B.y_d[4] = rtb_y_n[4]; 4837 } 4838 4839 /* S-Function "dsPIC_C_function_Call" Block: <S8>/Update PWM Commands [groundStationDriver.c] */ 4840 updatePWM(controlMCUSlugsMKII_B.y_d); 4841 4842 /* S-Function "dsPIC_C_function_Call" Block: <S8>/Update the Euler Angles [groundStationDriver.c] */ 4843 updateEuler(controlMCUSlugsMKII_B.sf_myMuxFun1_e.y_i); 4844 4845 /* S-Function "dsPIC_C_function_Call" Block: <S8>/Update PQR [groundStationDriver.c] */ 4846 updatePQR(controlMCUSlugsMKII_B.Product1); 4847 4848 /* S-Function "dsPIC_C_function_Call" Block: <S8>/Create Telemetry Sentence [groundStationDriver.c] */ 4849 prepareTelemetry(&controlMCUSlugsMKII_B.CreateTelemetrySentencegroundSt[0]); 4850 4851 /* S-Function "dsPIC_C_function_Call" Block: <S8>/Send Data to Radio Modem [groundStationDriver.c] */ 4852 send2GS(controlMCUSlugsMKII_B.CreateTelemetrySentencegroundSt); 4853 4854 /* Stateflow: '<S2>/myMux Fun2' */ 4855 controlMCUSlugsMK_myMuxFun1(rtb_Switch3_oe, rtb_PsiDotLimit, 4856 controlMCUSlugsMKII_B.PsiDotLimit, &controlMCUSlugsMKII_B.sf_myMuxFun2_l); 4857 4858 /* S-Function "dsPIC_C_function_Call" Block: <S2>/Diagnostics [navSupport.c] */ 4859 setDiagnosticFloat(controlMCUSlugsMKII_B.sf_myMuxFun2_l.y); 4860 4861 /* DataTypeConversion: '<S290>/Data Type Conversion4' */ 4862 controlMCUSlugsMKII_B.DataTypeConversion4 = (uint16_T) 4863 (controlMCUSlugsMKII_ConstB.Gain4 <= 65535.0 ? 4864 controlMCUSlugsMKII_ConstB.Gain4 >= 0.0 ? controlMCUSlugsMKII_ConstB.Gain4 : 4865 0.0 : 65535.0); 4866 4867 /* S-Function "dsPIC_C_function_Call" Block: <S290>/Update PWM Commands4 */ 4868 updatePWM2(controlMCUSlugsMKII_B.DataTypeConversion4,((uint8_T)4U)); 4869 4870 { 4871 real_T tmp; 4872 4873 /* Gain: '<S6>/Gain4' */ 4874 rtb_Abs2 = 100.0F * controlMCUSlugsMKII_B.DataTypeConversion; 4875 4876 /* DataTypeConversion: '<S280>/Data Type Conversion2' incorporates: 4877 * Abs: '<S6>/Abs4' 4878 */ 4879 tmp = fmod(floor((real_T)rt_ABS(rtb_Abs2)), 65536.0); 4880 if (tmp < 0.0) { 4881 tmp += 65536.0; 4882 } 4883 4884 controlMCUSlugsMKII_B.DataTypeConversion2_e = (uint16_T)tmp; 4885 } 4886 4887 /* S-Function "dsPIC_C_function_Call" Block: <S280>/Update PWM Commands1 */ 4888 updatePWM2(controlMCUSlugsMKII_B.DataTypeConversion2_e,((uint8_T)0U)); 4889 4890 { 4891 real_T tmp; 4892 4893 /* Gain: '<S6>/Gain1' */ 4894 rtb_Gain2_f = 5729.578125F * controlMCUSlugsMKII_B.DataTypeConversion_f; 4895 4896 /* DataTypeConversion: '<S280>/Data Type Conversion3' incorporates: 4897 * Abs: '<S6>/Abs' 4898 */ 4899 tmp = fmod(floor((real_T)rt_ABS(rtb_Gain2_f)), 65536.0); 4900 if (tmp < 0.0) { 4901 tmp += 65536.0; 4902 } 4903 4904 controlMCUSlugsMKII_B.DataTypeConversion3 = (uint16_T)tmp; 4905 } 4906 4907 /* S-Function "dsPIC_C_function_Call" Block: <S280>/Update PWM Commands2 */ 4908 updatePWM2(controlMCUSlugsMKII_B.DataTypeConversion3,((uint8_T)1U)); 4909 4910 { 4911 real_T tmp; 4912 4913 /* Gain: '<S6>/Gain3' */ 4914 rtb_Gain2_f = 5729.578125F * controlMCUSlugsMKII_B.DataTypeConversion_c; 4915 4916 /* DataTypeConversion: '<S280>/Data Type Conversion1' incorporates: 4917 * Abs: '<S6>/Abs1' 4918 */ 4919 tmp = fmod(floor((real_T)rt_ABS(rtb_Gain2_f)), 65536.0); 4920 if (tmp < 0.0) { 4921 tmp += 65536.0; 4922 } 4923 4924 controlMCUSlugsMKII_B.DataTypeConversion1 = (uint16_T)tmp; 4925 } 4926 4927 /* S-Function "dsPIC_C_function_Call" Block: <S280>/Update PWM Commands3 */ 4928 updatePWM2(controlMCUSlugsMKII_B.DataTypeConversion1,((uint8_T)2U)); 4929 4930 { 4931 real_T tmp; 4932 4933 /* Gain: '<S6>/Gain2' */ 4934 rtb_Gain2_f = 5729.578125F * controlMCUSlugsMKII_B.DataTypeConversion_b; 4935 4936 /* DataTypeConversion: '<S280>/Data Type Conversion4' incorporates: 4937 * Abs: '<S6>/Abs2' 4938 */ 4939 tmp = fmod(floor((real_T)rt_ABS(rtb_Gain2_f)), 65536.0); 4940 if (tmp < 0.0) { 4941 tmp += 65536.0; 4942 } 4943 4944 controlMCUSlugsMKII_B.DataTypeConversion4_p = (uint16_T)tmp; 4945 } 4946 4947 /* S-Function "dsPIC_C_function_Call" Block: <S280>/Update PWM Commands4 */ 4948 updatePWM2(controlMCUSlugsMKII_B.DataTypeConversion4_p,((uint8_T)3U)); 4949 4950 /* S-Function "dsPIC_C_function_Call" Block: <S5>/GetFilterOnOff */ 4951 controlMCUSlugsMKII_B.GetFilterOnOff = getFilterOnOff(); 4952 4953 /* DataTypeConversion: '<S5>/Data Type Conversion1' */ 4954 controlMCUSlugsMKII_B.DataTypeConversion1_e = 4955 (controlMCUSlugsMKII_B.GetFilterOnOff != 0U); 4956 4957 /* S-Function "dsPIC_Digital_OutputWrite" Block: <S5>/HIL Attitude On//Off Switch */ 4958 LATBbits.LATB6 = controlMCUSlugsMKII_B.DataTypeConversion1_e; 4959 4960 /* S-Function "dsPIC_Digital_OutputWrite" Block: <S5>/HIL On//Off Switch */ 4961 LATEbits.LATE9 = controlMCUSlugsMKII_B.DataTypeConversion1_e; 4962 4963 /* Integer/Tapped Delay Block: '<S6>/Integer Delay' 4964 */ 4965 controlMCUSlugsMKII_DWork.IntegerDelay_X_p = rtb_ScheduleLPF; 4966 4967 /* Integer/Tapped Delay Block: '<S278>/Integer Delay2' 4968 */ 4969 controlMCUSlugsMKII_DWork.IntegerDelay2_X_k = controlMCUSlugsMKII_B.IC_c; 4970 4971 /* Integer/Tapped Delay Block: '<S17>/Integer Delay3' 4972 */ 4973 controlMCUSlugsMKII_DWork.IntegerDelay3_X_j5 = rtb_Switch3_ov; 4974 4975 /* Integer/Tapped Delay Block: '<S19>/Integer Delay3' 4976 */ 4977 controlMCUSlugsMKII_DWork.IntegerDelay3_X_b = rtb_Switch3_oe; 4978 if (controlMCUSlugsMKII_DWork.L1OutputFeedbackControllerWithP == (int32_T) 4979 SUBSYS_ENABLED) { 4980 /* Update for enable system: '<S2>/L1 Output Feedback Controller With Projection Operator' */ 4981 4982 /* Update for UnitDelay: '<S33>/UD' */ 4983 controlMCUSlugsMKII_DWork.UD_DSTATE_l = controlMCUSlugsMKII_B.navSupportc_g 4984 [0]; 4985 4986 /* Integer/Tapped Delay Block: '<S24>/Integer Delay' 4987 */ 4988 controlMCUSlugsMKII_DWork.IntegerDelay_X_d = rtb_Switch3_im; 4989 4990 /* Integer/Tapped Delay Block: '<S24>/Integer Delay1' 4991 */ 4992 controlMCUSlugsMKII_DWork.IntegerDelay1_X_nc = rtb_Subtract_b; 4993 4994 /* Integer/Tapped Delay Block: '<S26>/Integer Delay3' 4995 */ 4996 controlMCUSlugsMKII_DWork.IntegerDelay3_X_cw = rtb_Switch3_cj; 4997 4998 /* Update for UnitDelay: '<S42>/UD' */ 4999 controlMCUSlugsMKII_DWork.UD_DSTATE_ld = controlMCUSlugsMKII_B.navSupportc_g 5000 [1]; 5001 5002 /* Integer/Tapped Delay Block: '<S28>/Integer Delay' 5003 */ 5004 controlMCUSlugsMKII_DWork.IntegerDelay_X_c = rtb_Switch3_ae; 5005 5006 /* Integer/Tapped Delay Block: '<S28>/Integer Delay1' 5007 */ 5008 controlMCUSlugsMKII_DWork.IntegerDelay1_X_a = rtb_Subtract_c; 5009 5010 /* Integer/Tapped Delay Block: '<S13>/Integer Delay' 5011 */ 5012 controlMCUSlugsMKII_DWork.IntegerDelay_X_no = rtb_PsiDotLimit; 5013 5014 /* Integer/Tapped Delay Block: '<S13>/Integer Delay1' 5015 */ 5016 controlMCUSlugsMKII_DWork.IntegerDelay1_X_b = rtb_Switch3_im; 5017 5018 /* Integer/Tapped Delay Block: '<S27>/Integer Delay3' 5019 */ 5020 controlMCUSlugsMKII_DWork.IntegerDelay3_X_gww = rtb_Switch3_ae; 5021 5022 /* Integer/Tapped Delay Block: '<S29>/Integer Delay' 5023 */ 5024 controlMCUSlugsMKII_DWork.IntegerDelay_X_f = rtb_Switch3_gq; 5025 5026 /* Integer/Tapped Delay Block: '<S29>/Integer Delay1' 5027 */ 5028 controlMCUSlugsMKII_DWork.IntegerDelay1_X_ny[0] = 5029 controlMCUSlugsMKII_DWork.IntegerDelay1_X_ny[1]; 5030 controlMCUSlugsMKII_DWork.IntegerDelay1_X_ny[1] = rtb_Switch3_gq; 5031 5032 /* Integer/Tapped Delay Block: '<S29>/Integer Delay2' 5033 */ 5034 controlMCUSlugsMKII_DWork.IntegerDelay2_X_m = rtb_Switch3_im; 5035 5036 /* Integer/Tapped Delay Block: '<S43>/Integer Delay3' 5037 */ 5038 controlMCUSlugsMKII_DWork.IntegerDelay3_X_oe = rtb_Switch3_gq; 5039 5040 /* Integer/Tapped Delay Block: '<S44>/Integer Delay3' 5041 */ 5042 controlMCUSlugsMKII_DWork.IntegerDelay3_X_oe1 = rtb_Switch3_im; 5043 } 5044 5045 /* Update absolute time */ 5046 controlMCUSlugsMKII_M->Timing.clockTick0++; 5047 } 5048 5049 /* Model step function for TID1 */ 5050 void controlMCUSlugsMKII_step1(void) /* Sample time: [1.0s, 0.0s] */ 5051 { 5052 /* (no output/update code required) */ 5053 } 5054 5055 void controlMCUSlugsMKII_step(int_T tid) 5056 { 5057 switch (tid) { 5058 case 0 : 5059 controlMCUSlugsMKII_step0(); 5060 break; 5061 5062 case 1 : 5063 controlMCUSlugsMKII_step1(); 5064 break; 5065 5066 default : 5067 break; 5068 } 5069 } 5070 5071 /* Model initialize function */ 5072 void controlMCUSlugsMKII_initialize(boolean_T firstTime) 5073 { 5074 (void)firstTime; 5075 5076 /* Registration code */ 5077 /* initialize real-time model */ 5078 (void) memset((char_T *)controlMCUSlugsMKII_M,0, 5079 sizeof(RT_MODEL_controlMCUSlugsMKII)); 5080 5081 /* block I/O */ 5082 (void) memset(((void *) &controlMCUSlugsMKII_B),0, 5083 sizeof(BlockIO_controlMCUSlugsMKII)); 5084 5085 { 5086 int_T i; 5087 void *pVoidBlockIORegion; 5088 pVoidBlockIORegion = (void *)(&controlMCUSlugsMKII_B.IC); 5089 for (i = 0; i < 2; i++) { 5090 ((real_T*)pVoidBlockIORegion)[i] = 0.0; 5091 } 5092 5093 pVoidBlockIORegion = (void *) 5094 (&controlMCUSlugsMKII_B.GetMidLevelCommandsnavSupportc[0]); 5095 for (i = 0; i < 83; i++) { 5096 ((real32_T*)pVoidBlockIORegion)[i] = 0.0F; 5097 } 5098 5099 pVoidBlockIORegion = (void *)(&controlMCUSlugsMKII_B.y_n[0]); 5100 for (i = 0; i < 113; i++) { 5101 ((real32_T*)pVoidBlockIORegion)[i] = 0.0F; 5102 } 5103 5104 pVoidBlockIORegion = (void *)(&controlMCUSlugsMKII_B.sf_myMuxFun1_e.y_i[0]); 5105 for (i = 0; i < 3; i++) { 5106 ((real32_T*)pVoidBlockIORegion)[i] = 0.0F; 5107 } 5108 5109 pVoidBlockIORegion = (void *)(&controlMCUSlugsMKII_B.sf_myMuxFun2_c.y_i[0]); 5110 for (i = 0; i < 3; i++) { 5111 ((real32_T*)pVoidBlockIORegion)[i] = 0.0F; 5112 } 5113 5114 pVoidBlockIORegion = (void *)(&controlMCUSlugsMKII_B.sf_myMuxFun1_a.y_i[0]); 5115 for (i = 0; i < 3; i++) { 5116 ((real32_T*)pVoidBlockIORegion)[i] = 0.0F; 5117 } 5118 5119 pVoidBlockIORegion = (void *)(&controlMCUSlugsMKII_B.sf_myMuxFun1_m.y_l[0]); 5120 for (i = 0; i < 9; i++) { 5121 ((real32_T*)pVoidBlockIORegion)[i] = 0.0F; 5122 } 5123 5124 pVoidBlockIORegion = (void *)(&controlMCUSlugsMKII_B.sf_myMuxFun2.y_i[0]); 5125 for (i = 0; i < 3; i++) { 5126 ((real32_T*)pVoidBlockIORegion)[i] = 0.0F; 5127 } 5128 5129 pVoidBlockIORegion = (void *)(&controlMCUSlugsMKII_B.sf_myMuxFun1.y_i[0]); 5130 for (i = 0; i < 3; i++) { 5131 ((real32_T*)pVoidBlockIORegion)[i] = 0.0F; 5132 } 5133 5134 pVoidBlockIORegion = (void *)(&controlMCUSlugsMKII_B.sf_myMuxFun1_n.y_l[0]); 5135 for (i = 0; i < 9; i++) { 5136 ((real32_T*)pVoidBlockIORegion)[i] = 0.0F; 5137 } 5138 5139 pVoidBlockIORegion = (void *)(&controlMCUSlugsMKII_B.sf_myMuxFun2_l.y[0]); 5140 for (i = 0; i < 3; i++) { 5141 ((real32_T*)pVoidBlockIORegion)[i] = 0.0F; 5142 } 5143 5144 pVoidBlockIORegion = (void *)(&controlMCUSlugsMKII_B.sf_myMuxFun1_o.y[0]); 5145 for (i = 0; i < 3; i++) { 5146 ((real32_T*)pVoidBlockIORegion)[i] = 0.0F; 5147 } 5148 5149 pVoidBlockIORegion = (void *)(&controlMCUSlugsMKII_B.sf_myMuxFun1_h.y[0]); 5150 for (i = 0; i < 3; i++) { 5151 ((real32_T*)pVoidBlockIORegion)[i] = 0.0F; 5152 } 5153 5154 ((real32_T*)&controlMCUSlugsMKII_B.sf_negprotect1.zpVal)[0] = 0.0F; 5155 ((real32_T*)&controlMCUSlugsMKII_B.sf_negprotect_h.zpVal_k)[0] = 0.0F; 5156 ((real32_T*)&controlMCUSlugsMKII_B.sf_negprotect_f.zpVal_k)[0] = 0.0F; 5157 ((real32_T*)&controlMCUSlugsMKII_B.sf_negprotect_fm.zpVal)[0] = 0.0F; 5158 ((real32_T*)&controlMCUSlugsMKII_B.sf_EmbeddedMATLABFunction_d.xDoty_j)[0] = 5159 0.0F; 5160 ((real32_T*)&controlMCUSlugsMKII_B.sf_negprotect_b.zpVal_k)[0] = 0.0F; 5161 ((real32_T*)&controlMCUSlugsMKII_B.sf_EmbeddedMATLABFunction_or.xDoty_j)[0] = 5162 0.0F; 5163 ((real32_T*)&controlMCUSlugsMKII_B.sf_negprotect_bp.zpVal_k)[0] = 0.0F; 5164 ((real32_T*)&controlMCUSlugsMKII_B.sf_EmbeddedMATLABFunction_hk.xDoty_j)[0] = 5165 0.0F; 5166 ((real32_T*)&controlMCUSlugsMKII_B.sf_negprotect_g.zpVal_k)[0] = 0.0F; 5167 ((real32_T*)&controlMCUSlugsMKII_B.sf_EmbeddedMATLABFunction_mg.xDoty_g)[0] = 5168 0.0F; 5169 ((real32_T*)&controlMCUSlugsMKII_B.sf_negprotect_l.zpVal_k)[0] = 0.0F; 5170 ((real32_T*)&controlMCUSlugsMKII_B.sf_EmbeddedMATLABFunction_n.xDoty)[0] = 5171 0.0F; 5172 ((real32_T*)&controlMCUSlugsMKII_B.sf_EmbeddedMATLABFunction_a.xDoty_g)[0] = 5173 0.0F; 5174 ((real32_T*)&controlMCUSlugsMKII_B.sf_negprotect_e.zpVal_k)[0] = 0.0F; 5175 ((real32_T*)&controlMCUSlugsMKII_B.sf_EmbeddedMATLABFunction_m.xDoty)[0] = 5176 0.0F; 5177 ((real32_T*)&controlMCUSlugsMKII_B.sf_negprotect_c.zpVal)[0] = 0.0F; 5178 ((real32_T*)&controlMCUSlugsMKII_B.sf_negprotect.zpVal)[0] = 0.0F; 5179 } 5180 5181 /* states (dwork) */ 5182 (void) memset((char_T *) &controlMCUSlugsMKII_DWork,0, 5183 sizeof(D_Work_controlMCUSlugsMKII)); 5184 5185 { 5186 int_T i; 5187 real_T *dwork_ptr = (real_T *) &controlMCUSlugsMKII_DWork.IntegerDelay2_X; 5188 for (i = 0; i < 24; i++) { 5189 dwork_ptr[i] = 0.0; 5190 } 5191 } 5192 5193 { 5194 int_T i; 5195 real32_T *dwork_ptr = (real32_T *) 5196 &controlMCUSlugsMKII_DWork.IntegerDelay_X_p; 5197 for (i = 0; i < 91; i++) { 5198 dwork_ptr[i] = 0.0F; 5199 } 5200 } 5201 5202 /* S-Function "dsPIC_MASTER" initialization Block: <Root>/Master */ 5203 /* 1 testfcy */ 5204 /* 4.0E+007 */ 5205 /* CONFIG TIMER 1 for scheduling steps */ 5206 ConfigIntTimer1(T1_INT_PRIOR_0 & T1_INT_OFF); 5207 T1CON = 0x8010 ; /* T1_PS_1_8 */ 5208 PR1 = 49999; 5209 5210 /* Configuration TIMER 2 */ 5211 ConfigIntTimer2(T2_INT_PRIOR_0 & T2_INT_OFF); 5212 OpenTimer2(T2_ON & T2_GATE_OFF & 5213 T2_PS_1_8 & T2_SOURCE_INT , 65535); 5214 5215 /* Configuration TRIS */ 5216 TRISB = 65471; 5217 TRISE = 65023; 5218 5219 /* Configuration ADCHS */ 5220 AD1PCFGL = (68); 5221 AD2PCFGL = (68); 5222 AD1PCFGH = ((2097220 >> 16) & 65535); 5223 5224 /* S-Function "dsPIC_PWM_IC" initialization Block: <S277>/Input Capture */ 5225 /* Initialise Input Capture */ 5226 /* config capture */ 5227 OpenCapture2(IC_IDLE_CON & IC_TIMER2_SRC & IC_INT_1CAPTURE & 5228 IC_EVERY_RISE_EDGE); 5229 ConfigIntCapture2(IC_INT_ON & IC_INT_PRIOR_6); 5230 5231 /* config capture */ 5232 OpenCapture3(IC_IDLE_CON & IC_TIMER2_SRC & IC_INT_1CAPTURE & 5233 IC_EVERY_RISE_EDGE); 5234 ConfigIntCapture3(IC_INT_ON & IC_INT_PRIOR_6); 5235 5236 /* config capture */ 5237 OpenCapture4(IC_IDLE_CON & IC_TIMER2_SRC & IC_INT_1CAPTURE & 5238 IC_EVERY_RISE_EDGE); 5239 ConfigIntCapture4(IC_INT_ON & IC_INT_PRIOR_6); 5240 5241 /* config capture */ 5242 OpenCapture5(IC_IDLE_CON & IC_TIMER2_SRC & IC_INT_1CAPTURE & 5243 IC_EVERY_RISE_EDGE); 5244 ConfigIntCapture5(IC_INT_ON & IC_INT_PRIOR_6); 5245 5246 /* config capture */ 5247 OpenCapture8(IC_IDLE_CON & IC_TIMER2_SRC & IC_INT_1CAPTURE & 5248 IC_EVERY_RISE_EDGE); 5249 ConfigIntCapture8(IC_INT_ON & IC_INT_PRIOR_6); 5250 5251 /* Start for InitialCondition: '<S278>/IC' */ 5252 controlMCUSlugsMKII_B.IC_c = 1U; 5253 controlMCUSlugsMKII_DWork.IC_FirstOutputTime = 1U; 5254 5255 /* Start for atomic system: '<S2>/Navigation' */ 5256 5257 /* Start for InitialCondition: '<S124>/IC1' */ 5258 controlMCUSlugsMKII_DWork.IC1_FirstOutputTime = 1U; 5259 5260 /* Start for InitialCondition: '<S139>/IC' */ 5261 controlMCUSlugsMKII_B.IC = 1.0; 5262 controlMCUSlugsMKII_DWork.IC_FirstOutputTime_i = 1U; 5263 5264 /* Start for InitialCondition: '<S180>/IC' */ 5265 controlMCUSlugsMKII_B.IC_e = 1.0F; 5266 controlMCUSlugsMKII_DWork.IC_FirstOutputTime_b = 1U; 5267 5268 /* Start for InitialCondition: '<S140>/IC' */ 5269 controlMCUSlugsMKII_DWork.IC_FirstOutputTime_d = 1U; 5270 5271 /* Start for enable system: '<S140>/Compute Total Run' */ 5272 5273 /* virtual outports code */ 5274 5275 /* (Virtual) Outport Block: '<S179>/Tot Run' */ 5276 controlMCUSlugsMKII_B.Switch3 = 150.0F; 5277 5278 /* Start for enable system: '<S124>/Compute P' */ 5279 5280 /* Start for InitialCondition: '<S133>/IC' */ 5281 controlMCUSlugsMKII_DWork.IC_FirstOutputTime_k = 1U; 5282 5283 /* virtual outports code */ 5284 5285 /* (Virtual) Outport Block: '<S133>/P' */ 5286 controlMCUSlugsMKII_B.IC_l = 50.0F; 5287 5288 /* Start for enable system: '<S2>/L1 Output Feedback Controller With Projection Operator' */ 5289 5290 /* Start for InitialCondition: '<S24>/IC' */ 5291 controlMCUSlugsMKII_DWork.IC_FirstOutputTime_g = 1U; 5292 5293 /* Start for InitialCondition: '<S28>/IC' */ 5294 controlMCUSlugsMKII_DWork.IC_FirstOutputTime_p = 1U; 5295 5296 /* Initial conditions for enable system: '<S2>/L1 Output Feedback Controller With Projection Operator' */ 5297 5298 /* InitializeConditions for UnitDelay: '<S33>/UD' */ 5299 controlMCUSlugsMKII_DWork.UD_DSTATE_l = 0.0F; 5300 5301 /* Integer/Tapped Delay Block: '<S24>/Integer Delay' 5302 * Initialize State 5303 * Initial Condition parameter uses the same data type and scaling as Output0 5304 */ 5305 controlMCUSlugsMKII_DWork.IntegerDelay_X_d = (0.0F); 5306 5307 /* Integer/Tapped Delay Block: '<S24>/Integer Delay1' 5308 * Initialize State 5309 * Initial Condition parameter uses the same data type and scaling as Output0 5310 */ 5311 controlMCUSlugsMKII_DWork.IntegerDelay1_X_nc = (0.0F); 5312 5313 /* Integer/Tapped Delay Block: '<S26>/Integer Delay3' 5314 * Initialize State 5315 * Initial Condition parameter uses the same data type and scaling as Output0 5316 */ 5317 controlMCUSlugsMKII_DWork.IntegerDelay3_X_cw = (0.0F); 5318 5319 /* InitializeConditions for UnitDelay: '<S42>/UD' */ 5320 controlMCUSlugsMKII_DWork.UD_DSTATE_ld = 0.0F; 5321 5322 /* Integer/Tapped Delay Block: '<S28>/Integer Delay' 5323 * Initialize State 5324 * Initial Condition parameter uses the same data type and scaling as Output0 5325 */ 5326 controlMCUSlugsMKII_DWork.IntegerDelay_X_c = (0.0F); 5327 5328 /* Integer/Tapped Delay Block: '<S28>/Integer Delay1' 5329 * Initialize State 5330 * Initial Condition parameter uses the same data type and scaling as Output0 5331 */ 5332 controlMCUSlugsMKII_DWork.IntegerDelay1_X_a = (0.0F); 5333 5334 /* Integer/Tapped Delay Block: '<S13>/Integer Delay' 5335 * Initialize State 5336 * Initial Condition parameter uses the same data type and scaling as Output0 5337 */ 5338 controlMCUSlugsMKII_DWork.IntegerDelay_X_no = (0.0F); 5339 5340 /* Integer/Tapped Delay Block: '<S13>/Integer Delay1' 5341 * Initialize State 5342 * Initial Condition parameter uses the same data type and scaling as Output0 5343 */ 5344 controlMCUSlugsMKII_DWork.IntegerDelay1_X_b = (0.0F); 5345 5346 /* InitializeConditions for Merge: '<S25>/Merge' */ 5347 if (rtmIsFirstInitCond(controlMCUSlugsMKII_M)) { 5348 controlMCUSlugsMKII_B.Merge = 0.0F; 5349 } 5350 5351 /* Integer/Tapped Delay Block: '<S27>/Integer Delay3' 5352 * Initialize State 5353 * Initial Condition parameter uses the same data type and scaling as Output0 5354 */ 5355 controlMCUSlugsMKII_DWork.IntegerDelay3_X_gww = (0.0F); 5356 5357 /* Integer/Tapped Delay Block: '<S29>/Integer Delay' 5358 * Initialize State 5359 * Initial Condition parameter uses the same data type and scaling as Output0 5360 */ 5361 controlMCUSlugsMKII_DWork.IntegerDelay_X_f = (0.0F); 5362 5363 /* Integer/Tapped Delay Block: '<S29>/Integer Delay1' 5364 * Initialize State 5365 * Initial Condition parameter uses the same data type and scaling as Output0 5366 */ 5367 controlMCUSlugsMKII_DWork.IntegerDelay1_X_ny[0] = (0.0F); 5368 controlMCUSlugsMKII_DWork.IntegerDelay1_X_ny[1] = (0.0F); 5369 5370 /* Integer/Tapped Delay Block: '<S29>/Integer Delay2' 5371 * Initialize State 5372 * Initial Condition parameter uses the same data type and scaling as Output0 5373 */ 5374 controlMCUSlugsMKII_DWork.IntegerDelay2_X_m = (0.0F); 5375 5376 /* Integer/Tapped Delay Block: '<S43>/Integer Delay3' 5377 * Initialize State 5378 * Initial Condition parameter uses the same data type and scaling as Output0 5379 */ 5380 controlMCUSlugsMKII_DWork.IntegerDelay3_X_oe = (0.0F); 5381 5382 /* Integer/Tapped Delay Block: '<S44>/Integer Delay3' 5383 * Initialize State 5384 * Initial Condition parameter uses the same data type and scaling as Output0 5385 */ 5386 controlMCUSlugsMKII_DWork.IntegerDelay3_X_oe1 = (0.0F); 5387 5388 /* Start for atomic system: '<S2>/Lateral Channel' */ 5389 5390 /* Start for enable system: '<S14>/Sideslip Compensation' */ 5391 5392 /* Start for InitialCondition: '<S71>/IC' */ 5393 controlMCUSlugsMKII_DWork.IC_FirstOutputTime_e = 1U; 5394 5395 /* Initial conditions for enable system: '<S14>/Sideslip Compensation' */ 5396 5397 /* InitializeConditions for UnitDelay: '<S79>/UD' */ 5398 controlMCUSlugsMKII_DWork.UD_DSTATE_o = 0.0F; 5399 5400 /* Integer/Tapped Delay Block: '<S71>/Integer Delay' 5401 * Initialize State 5402 * Initial Condition parameter uses the same data type and scaling as Output0 5403 */ 5404 controlMCUSlugsMKII_DWork.IntegerDelay_X_b = (0.0F); 5405 5406 /* Integer/Tapped Delay Block: '<S71>/Integer Delay1' 5407 * Initialize State 5408 * Initial Condition parameter uses the same data type and scaling as Output0 5409 */ 5410 controlMCUSlugsMKII_DWork.IntegerDelay1_X_f5 = (0.0); 5411 5412 /* Integer/Tapped Delay Block: '<S72>/Integer Delay3' 5413 * Initialize State 5414 * Initial Condition parameter uses the same data type and scaling as Output0 5415 */ 5416 controlMCUSlugsMKII_DWork.IntegerDelay3_X_hc = (0.0F); 5417 5418 /* S-Function "dsPIC_PWM_motor" initialization Block: <S8>/PWM Servo Output */ 5419 /* Initialise Output Capture */ 5420 SetDCMCPWM(1,0,0); 5421 SetDCMCPWM(2,0,0); 5422 SetDCMCPWM(3,0,0); 5423 SetDCMCPWM(4,0,0); 5424 5425 #define config2 (0xffff &PWM_PEN1L & PWM_MOD1_IND & PWM_PDIS1H &PWM_PEN2L & PWM_MOD2_IND & PWM_PDIS2H &PWM_PEN3L & PWM_MOD3_IND & PWM_PDIS3H &PWM_PEN4L & PWM_MOD4_IND & PWM_PDIS4H) 5426 #define config (PWM_INT_DIS & PWM_FLTA_DIS_INT & PWM_INT_PR1 & PWM_FLTA_INT_PR0) 5427 5428 ConfigIntMCPWM( config ); 5429 5430 #define config1 (PWM_EN & 0xDFFF & PWM_OP_SCALE1 & PWM_IPCLK_SCALE16 & PWM_MOD_UPDN) 5431 #define config3 (PWM_SEVOPS1 & PWM_OSYNC_TCY & PWM_UEN) 5432 5433 OpenMCPWM(25000,0,config1,config2,config3); 5434 5435 /* blocks with infinite sample periods */ 5436 5437 /* S-Function "dsPIC_C_function_Call" Block: <Root>/Initialize the MCU [groundStationDriver.c] */ 5438 controlMCUInit(); 5439 controlMCUSlugsM_PrevZCSigState.HitCrossing_ZCE = UNINITIALIZED_ZCSIG; 5440 controlMCUSlugsM_PrevZCSigState.HitCrossing1_ZCE = UNINITIALIZED_ZCSIG; 5441 5442 /* Initialize code for chart: '<S276>/Buffer IC Channel' */ 5443 { 5444 int16_T eml_i0; 5445 for (eml_i0 = 0; eml_i0 < 7; eml_i0++) { 5446 controlMCUSlugsMKII_DWork.BufferICChannel.oldValues[eml_i0] = 0U; 5447 } 5448 } 5449 5450 /* Initialize code for chart: '<S276>/Buffer IC Channel1' */ 5451 { 5452 int16_T eml_i0; 5453 for (eml_i0 = 0; eml_i0 < 7; eml_i0++) { 5454 controlMCUSlugsMKII_DWork.BufferICChannel1.oldValues[eml_i0] = 0U; 5455 } 5456 } 5457 5458 /* Initialize code for chart: '<S276>/Buffer IC Channel2' */ 5459 { 5460 int16_T eml_i0; 5461 for (eml_i0 = 0; eml_i0 < 7; eml_i0++) { 5462 controlMCUSlugsMKII_DWork.BufferICChannel2.oldValues[eml_i0] = 0U; 5463 } 5464 } 5465 5466 /* Initialize code for chart: '<S276>/Buffer IC Channel3' */ 5467 { 5468 int16_T eml_i0; 5469 for (eml_i0 = 0; eml_i0 < 7; eml_i0++) { 5470 controlMCUSlugsMKII_DWork.BufferICChannel3.oldValues[eml_i0] = 0U; 5471 } 5472 } 5473 5474 /* Initial conditions for atomic system: '<S2>/Navigation' */ 5475 5476 /* Integer/Tapped Delay Block: '<S124>/Integer Delay2' 5477 * Initialize State 5478 * Initial Condition parameter uses the same data type and scaling as Output0 5479 */ 5480 controlMCUSlugsMKII_DWork.IntegerDelay2_X = (1.0); 5481 5482 /* Initialize code for chart: '<S124>/Embedded MATLAB Function' */ 5483 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_g.fromWP_not_empty = false; 5484 5485 /* Integer/Tapped Delay Block: '<S124>/Integer Delay3' 5486 * Initialize State 5487 * Initial Condition parameter uses the same data type and scaling as Output0 5488 */ 5489 controlMCUSlugsMKII_DWork.IntegerDelay3_X_a = (50.0F); 5490 5491 /* Integer/Tapped Delay Block: '<S180>/Integer Delay1' 5492 * Initialize State 5493 * Initial Condition parameter uses the same data type and scaling as Output0 5494 */ 5495 controlMCUSlugsMKII_DWork.IntegerDelay1_X_i = (1.0F); 5496 5497 /* Integer/Tapped Delay Block: '<S180>/Integer Delay2' 5498 * Initialize State 5499 * Initial Condition parameter uses the same data type and scaling as Output0 5500 */ 5501 controlMCUSlugsMKII_DWork.IntegerDelay2_X_e[0] = (1.0F); 5502 controlMCUSlugsMKII_DWork.IntegerDelay2_X_e[1] = (1.0F); 5503 5504 /* Initialize code for chart: '<S124>/Embedded MATLAB Function1' */ 5505 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction1.internalTimer = 0U; 5506 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction1.oldSw = 0U; 5507 5508 /* Initial conditions for atomic system: '<S2>/Longitudinal Channel' */ 5509 5510 /* Initialize code for chart: '<S88>/Embedded MATLAB Function' */ 5511 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_m.a_not_empty = false; 5512 5513 /* Initialize code for chart: '<S90>/Embedded MATLAB Function' */ 5514 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_l.a_not_empty = false; 5515 5516 /* InitializeConditions for RateLimiter: '<S15>/Rate Limiter' */ 5517 controlMCUSlugsMKII_DWork.PrevY = 180.0F; 5518 5519 /* InitializeConditions for Memory: '<S94>/Memory1' */ 5520 controlMCUSlugsMKII_DWork.Memory1_PreviousInput = 0.0F; 5521 5522 /* Initialize code for chart: '<S89>/Embedded MATLAB Function' */ 5523 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction_j.a_not_empty = false; 5524 5525 /* InitializeConditions for Memory: '<S92>/Memory1' */ 5526 controlMCUSlugsMKII_DWork.Memory1_PreviousInput_e = 0.0F; 5527 5528 /* InitializeConditions for Memory: '<S95>/Memory1' */ 5529 controlMCUSlugsMKII_DWork.Memory1_PreviousInput_f = 0.0F; 5530 5531 /* Initial conditions for atomic system: '<S2>/Lateral Channel' */ 5532 5533 /* Initialize code for chart: '<S45>/Embedded MATLAB Function' */ 5534 controlMCUSlugsMKII_DWork.EmbeddedMATLABFunction.a_not_empty = false; 5535 5536 /* InitializeConditions for Memory: '<S57>/Memory1' */ 5537 controlMCUSlugsMKII_DWork.Memory1_PreviousInput_a = 0.0F; 5538 5539 /* InitializeConditions for Memory: '<S55>/Memory1' */ 5540 controlMCUSlugsMKII_DWork.Memory1_PreviousInput_l = 0.0F; 5541 } 5542 5543 /* File trailer for Real-Time Workshop generated code. 5544 * 5545 * [EOF] 5546 */