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