File: C:\Mariano\Temp\SLUG_Autopilot\Stable\controlMCUSlugsMKII_dspic\sources\controlMCUSlugsMKII.c

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