File: C:\Users\SLUG\Documents\MATLAB\SLUG_Autopilot\Stable\controlMCUSlugsMKII_dspic\sources\controlMCUSlugsMKII.c

    1   /*
    2    * File: controlMCUSlugsMKII.c
    3    *
    4    * Real-Time Workshop code generated for Simulink model controlMCUSlugsMKII.
    5    *
    6    * Model version                        : 1.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