File: Controller.cpp

    1   /*
    2    * Controller.cpp
    3    *
    4    * Code generation for model "Controller".
    5    *
    6    * Model version              : 1.1303
    7    * Simulink Coder version : 8.3 (R2012b) 20-Jul-2012
    8    * C++ source code generated on : Sun Jun 01 14:58:10 2014
    9    *
   10    * Target selection: grt.tlc
   11    * Note: GRT includes extra infrastructure and instrumentation for prototyping
   12    * Embedded hardware selection: Generic->32-bit x86 compatible
   13    * Code generation objectives: Unspecified
   14    * Validation result: Not run
   15    */
   16   #include "Controller.h"
   17   #include "Controller_private.h"
   18   
   19   /* Block signals (auto storage) */
   20   BlockIO_Controller Controller_B;
   21   
   22   /* Continuous states */
   23   ContinuousStates_Controller Controller_X;
   24   
   25   /* Block states (auto storage) */
   26   D_Work_Controller Controller_DWork;
   27   
   28   /* Previous zero-crossings (trigger) states */
   29   PrevZCSigStates_Controller Controller_PrevZCSigState;
   30   
   31   /* External inputs (root inport signals with auto storage) */
   32   ExternalInputs_Controller Controller_U;
   33   
   34   /* External outputs (root outports fed by signals with auto storage) */
   35   ExternalOutputs_Controller Controller_Y;
   36   
   37   /* Real-time model */
   38   RT_MODEL_Controller Controller_M_;
   39   RT_MODEL_Controller *const Controller_M = &Controller_M_;
   40   
   41   /* Forward declaration for local functions */
   42   static real_T Controller_sum(const real_T x[6]);
   43   static void Controller_diag(const real_T v[12], real_T d[144]);
   44   static void Controller_invNxN(const real_T x[144], real_T y[144]);
   45   static void Controller_inv(const real_T x[144], real_T y[144]);
   46   static real_T Controller_inv_i(real_T x);
   47   static void Controller_eye(real_T I[144]);
   48   
   49   /* Forward declaration for local functions */
   50   static void Controller_conv(const real_T A[2], const real_T B[3], real_T C[4]);
   51   
   52   /*
   53    * Time delay interpolation routine
   54    *
   55    * The linear interpolation is performed using the formula:
   56    *
   57    *          (t2 - tMinusDelay)         (tMinusDelay - t1)
   58    * u(t)  =  ----------------- * u1  +  ------------------- * u2
   59    *              (t2 - t1)                  (t2 - t1)
   60    */
   61   real_T rt_TDelayInterpolate(
   62     real_T tMinusDelay,                  /* tMinusDelay = currentSimTime - delay */
   63     real_T tStart,
   64     real_T *tBuf,
   65     real_T *uBuf,
   66     int_T bufSz,
   67     int_T *lastIdx,
   68     int_T oldestIdx,
   69     int_T newIdx,
   70     real_T initOutput,
   71     boolean_T discrete,
   72     boolean_T minorStepAndTAtLastMajorOutput)
   73   {
   74     int_T i;
   75     real_T yout, t1, t2, u1, u2;
   76   
   77     /*
   78      * If there is only one data point in the buffer, this data point must be
   79      * the t= 0 and tMinusDelay > t0, it ask for something unknown. The best
   80      * guess if initial output as well
   81      */
   82     if ((newIdx == 0) && (oldestIdx ==0 ) && (tMinusDelay > tStart))
   83       return initOutput;
   84   
   85     /*
   86      * If tMinusDelay is less than zero, should output initial value
   87      */
   88     if (tMinusDelay <= tStart)
   89       return initOutput;
   90   
   91     /* For fixed buffer extrapolation:
   92      * if tMinusDelay is small than the time at oldestIdx, if discrete, output
   93      * tailptr value,  else use tailptr and tailptr+1 value to extrapolate
   94      * It is also for fixed buffer. Note: The same condition can happen for transport delay block where
   95      * use tStart and and t[tail] other than using t[tail] and t[tail+1].
   96      * See below
   97      */
   98     if ((tMinusDelay <= tBuf[oldestIdx] ) ) {
   99       if (discrete) {
  100         return(uBuf[oldestIdx]);
  101       } else {
  102         int_T tempIdx= oldestIdx + 1;
  103         if (oldestIdx == bufSz-1)
  104           tempIdx = 0;
  105         t1= tBuf[oldestIdx];
  106         t2= tBuf[tempIdx];
  107         u1= uBuf[oldestIdx];
  108         u2= uBuf[tempIdx];
  109         if (t2 == t1) {
  110           if (tMinusDelay >= t2) {
  111             yout = u2;
  112           } else {
  113             yout = u1;
  114           }
  115         } else {
  116           real_T f1 = (t2-tMinusDelay) / (t2-t1);
  117           real_T f2 = 1.0 - f1;
  118   
  119           /*
  120            * Use Lagrange's interpolation formula.  Exact outputs at t1, t2.
  121            */
  122           yout = f1*u1 + f2*u2;
  123         }
  124   
  125         return yout;
  126       }
  127     }
  128   
  129     /*
  130      * When block does not have direct feedthrough, we use the table of
  131      * values to extrapolate off the end of the table for delays that are less
  132      * than 0 (less then step size).  This is not completely accurate.  The
  133      * chain of events is as follows for a given time t.  Major output - look
  134      * in table.  Update - add entry to table.  Now, if we call the output at
  135      * time t again, there is a new entry in the table. For very small delays,
  136      * this means that we will have a different answer from the previous call
  137      * to the output fcn at the same time t.  The following code prevents this
  138      * from happening.
  139      */
  140     if (minorStepAndTAtLastMajorOutput) {
  141       /* pretend that the new entry has not been added to table */
  142       if (newIdx != 0) {
  143         if (*lastIdx == newIdx) {
  144           (*lastIdx)--;
  145         }
  146   
  147         newIdx--;
  148       } else {
  149         if (*lastIdx == newIdx) {
  150           *lastIdx = bufSz-1;
  151         }
  152   
  153         newIdx = bufSz - 1;
  154       }
  155     }
  156   
  157     i = *lastIdx;
  158     if (tBuf[i] < tMinusDelay) {
  159       /* Look forward starting at last index */
  160       while (tBuf[i] < tMinusDelay) {
  161         /* May occur if the delay is less than step-size - extrapolate */
  162         if (i == newIdx)
  163           break;
  164         i = ( i < (bufSz-1) ) ? (i+1) : 0;/* move through buffer */
  165       }
  166     } else {
  167       /*
  168        * Look backwards starting at last index which can happen when the
  169        * delay time increases.
  170        */
  171       while (tBuf[i] >= tMinusDelay) {
  172         /*
  173          * Due to the entry condition at top of function, we
  174          * should never hit the end.
  175          */
  176         i = (i > 0) ? i-1 : (bufSz-1);   /* move through buffer */
  177       }
  178   
  179       i = ( i < (bufSz-1) ) ? (i+1) : 0;
  180     }
  181   
  182     *lastIdx = i;
  183     if (discrete) {
  184       /*
  185        * tempEps = 128 * eps;
  186        * localEps = max(tempEps, tempEps*fabs(tBuf[i]))/2;
  187        */
  188       double tempEps = (DBL_EPSILON) * 128.0;
  189       double localEps = tempEps * fabs(tBuf[i]);
  190       if (tempEps > localEps) {
  191         localEps = tempEps;
  192       }
  193   
  194       localEps = localEps / 2.0;
  195       if (tMinusDelay >= (tBuf[i] - localEps)) {
  196         yout = uBuf[i];
  197       } else {
  198         if (i == 0) {
  199           yout = uBuf[bufSz-1];
  200         } else {
  201           yout = uBuf[i-1];
  202         }
  203       }
  204     } else {
  205       if (i == 0) {
  206         t1 = tBuf[bufSz-1];
  207         u1 = uBuf[bufSz-1];
  208       } else {
  209         t1 = tBuf[i-1];
  210         u1 = uBuf[i-1];
  211       }
  212   
  213       t2 = tBuf[i];
  214       u2 = uBuf[i];
  215       if (t2 == t1) {
  216         if (tMinusDelay >= t2) {
  217           yout = u2;
  218         } else {
  219           yout = u1;
  220         }
  221       } else {
  222         real_T f1 = (t2-tMinusDelay) / (t2-t1);
  223         real_T f2 = 1.0 - f1;
  224   
  225         /*
  226          * Use Lagrange's interpolation formula.  Exact outputs at t1, t2.
  227          */
  228         yout = f1*u1 + f2*u2;
  229       }
  230     }
  231   
  232     return(yout);
  233   }
  234   
  235   /*
  236    * This function updates continuous states using the ODE4 fixed-step
  237    * solver algorithm
  238    */
  239   static void rt_ertODEUpdateContinuousStates(RTWSolverInfo *si )
  240   {
  241     time_T t = rtsiGetT(si);
  242     time_T tnew = rtsiGetSolverStopTime(si);
  243     time_T h = rtsiGetStepSize(si);
  244     real_T *x = rtsiGetContStates(si);
  245     ODE4_IntgData *id = (ODE4_IntgData *)rtsiGetSolverData(si);
  246     real_T *y = id->y;
  247     real_T *f0 = id->f[0];
  248     real_T *f1 = id->f[1];
  249     real_T *f2 = id->f[2];
  250     real_T *f3 = id->f[3];
  251     real_T temp;
  252     int_T i;
  253     int_T nXc = 198;
  254     rtsiSetSimTimeStep(si,MINOR_TIME_STEP);
  255   
  256     /* Save the state values at time t in y, we'll use x as ynew. */
  257     (void) memcpy(y, x,
  258                   (uint_T)nXc*sizeof(real_T));
  259   
  260     /* Assumes that rtsiSetT and ModelOutputs are up-to-date */
  261     /* f0 = f(t,y) */
  262     rtsiSetdX(si, f0);
  263     Controller_derivatives();
  264   
  265     /* f1 = f(t + (h/2), y + (h/2)*f0) */
  266     temp = 0.5 * h;
  267     for (i = 0; i < nXc; i++) {
  268       x[i] = y[i] + (temp*f0[i]);
  269     }
  270   
  271     rtsiSetT(si, t + temp);
  272     rtsiSetdX(si, f1);
  273     Controller_step();
  274     Controller_derivatives();
  275   
  276     /* f2 = f(t + (h/2), y + (h/2)*f1) */
  277     for (i = 0; i < nXc; i++) {
  278       x[i] = y[i] + (temp*f1[i]);
  279     }
  280   
  281     rtsiSetdX(si, f2);
  282     Controller_step();
  283     Controller_derivatives();
  284   
  285     /* f3 = f(t + h, y + h*f2) */
  286     for (i = 0; i < nXc; i++) {
  287       x[i] = y[i] + (h*f2[i]);
  288     }
  289   
  290     rtsiSetT(si, tnew);
  291     rtsiSetdX(si, f3);
  292     Controller_step();
  293     Controller_derivatives();
  294   
  295     /* tnew = t + h
  296        ynew = y + (h/6)*(f0 + 2*f1 + 2*f2 + 2*f3) */
  297     temp = h / 6.0;
  298     for (i = 0; i < nXc; i++) {
  299       x[i] = y[i] + temp*(f0[i] + 2.0*f1[i] + 2.0*f2[i] + f3[i]);
  300     }
  301   
  302     rtsiSetSimTimeStep(si,MAJOR_TIME_STEP);
  303   }
  304   
  305   /* Function for MATLAB Function: '<S14>/MATLAB Function' */
  306   static real_T Controller_sum(const real_T x[6])
  307   {
  308     real_T y;
  309     int32_T k;
  310     y = x[0];
  311     for (k = 0; k < 5; k++) {
  312       y += x[k + 1];
  313     }
  314   
  315     return y;
  316   }
  317   
  318   /* Function for MATLAB Function: '<S14>/MATLAB Function' */
  319   static void Controller_diag(const real_T v[12], real_T d[144])
  320   {
  321     int32_T j;
  322     memset(&d[0], 0, 144U * sizeof(real_T));
  323     for (j = 0; j < 12; j++) {
  324       d[j + 12 * j] = v[j];
  325     }
  326   }
  327   
  328   /* Function for MATLAB Function: '<S14>/MATLAB Function' */
  329   static void Controller_invNxN(const real_T x[144], real_T y[144])
  330   {
  331     int8_T p[12];
  332     real_T A[144];
  333     int8_T ipiv[12];
  334     int32_T b_j;
  335     int32_T c;
  336     int32_T ix;
  337     real_T smax;
  338     real_T s;
  339     int32_T b_ix;
  340     int32_T jy;
  341     int32_T ijA;
  342     int32_T pipk;
  343     int32_T c_i;
  344     for (b_j = 0; b_j < 144; b_j++) {
  345       y[b_j] = 0.0;
  346       A[b_j] = x[b_j];
  347     }
  348   
  349     for (b_j = 0; b_j < 12; b_j++) {
  350       ipiv[b_j] = (int8_T)(1 + b_j);
  351     }
  352   
  353     for (b_j = 0; b_j < 11; b_j++) {
  354       c = b_j * 13;
  355       jy = 0;
  356       ix = c;
  357       smax = fabs(A[c]);
  358       for (pipk = 2; pipk <= 12 - b_j; pipk++) {
  359         ix++;
  360         s = fabs(A[ix]);
  361         if (s > smax) {
  362           jy = pipk - 1;
  363           smax = s;
  364         }
  365       }
  366   
  367       if (A[c + jy] != 0.0) {
  368         if (jy != 0) {
  369           ipiv[b_j] = (int8_T)((b_j + jy) + 1);
  370           b_ix = b_j;
  371           pipk = b_j + jy;
  372           for (jy = 0; jy < 12; jy++) {
  373             smax = A[b_ix];
  374             A[b_ix] = A[pipk];
  375             A[pipk] = smax;
  376             b_ix += 12;
  377             pipk += 12;
  378           }
  379         }
  380   
  381         ix = (c - b_j) + 12;
  382         for (pipk = c + 1; pipk + 1 <= ix; pipk++) {
  383           A[pipk] /= A[c];
  384         }
  385       }
  386   
  387       pipk = c;
  388       jy = c + 12;
  389       for (c_i = 1; c_i <= 11 - b_j; c_i++) {
  390         smax = A[jy];
  391         if (A[jy] != 0.0) {
  392           ix = c + 1;
  393           b_ix = (pipk - b_j) + 24;
  394           for (ijA = 13 + pipk; ijA + 1 <= b_ix; ijA++) {
  395             A[ijA] += A[ix] * -smax;
  396             ix++;
  397           }
  398         }
  399   
  400         jy += 12;
  401         pipk += 12;
  402       }
  403     }
  404   
  405     for (b_j = 0; b_j < 12; b_j++) {
  406       p[b_j] = (int8_T)(1 + b_j);
  407     }
  408   
  409     for (ix = 0; ix < 11; ix++) {
  410       if (ipiv[ix] > 1 + ix) {
  411         pipk = p[ipiv[ix] - 1];
  412         p[ipiv[ix] - 1] = p[ix];
  413         p[ix] = (int8_T)pipk;
  414       }
  415     }
  416   
  417     for (ix = 0; ix < 12; ix++) {
  418       y[ix + 12 * (p[ix] - 1)] = 1.0;
  419       for (pipk = ix; pipk + 1 < 13; pipk++) {
  420         if (y[(p[ix] - 1) * 12 + pipk] != 0.0) {
  421           for (jy = pipk + 1; jy + 1 < 13; jy++) {
  422             y[jy + 12 * (p[ix] - 1)] = y[(p[ix] - 1) * 12 + jy] - y[(p[ix] - 1) *
  423               12 + pipk] * A[12 * pipk + jy];
  424           }
  425         }
  426       }
  427     }
  428   
  429     for (ix = 0; ix < 12; ix++) {
  430       pipk = 12 * ix;
  431       for (jy = 11; jy >= 0; jy += -1) {
  432         b_ix = 12 * jy;
  433         if (y[jy + pipk] != 0.0) {
  434           y[jy + pipk] /= A[jy + b_ix];
  435           for (c_i = 0; c_i + 1 <= jy; c_i++) {
  436             y[c_i + pipk] -= y[jy + pipk] * A[c_i + b_ix];
  437           }
  438         }
  439       }
  440     }
  441   }
  442   
  443   /* Function for MATLAB Function: '<S14>/MATLAB Function' */
  444   static void Controller_inv(const real_T x[144], real_T y[144])
  445   {
  446     Controller_invNxN(x, y);
  447   }
  448   
  449   /* Function for MATLAB Function: '<S14>/MATLAB Function' */
  450   static real_T Controller_inv_i(real_T x)
  451   {
  452     return 1.0 / x;
  453   }
  454   
  455   /* Function for MATLAB Function: '<S14>/MATLAB Function' */
  456   static void Controller_eye(real_T I[144])
  457   {
  458     int32_T i;
  459     memset(&I[0], 0, 144U * sizeof(real_T));
  460     for (i = 0; i < 12; i++) {
  461       I[i + 12 * i] = 1.0;
  462     }
  463   }
  464   
  465   real_T rt_powd_snf(real_T u0, real_T u1)
  466   {
  467     real_T y;
  468     real_T tmp;
  469     real_T tmp_0;
  470     if (rtIsNaN(u0) || rtIsNaN(u1)) {
  471       y = (rtNaN);
  472     } else {
  473       tmp = fabs(u0);
  474       tmp_0 = fabs(u1);
  475       if (rtIsInf(u1)) {
  476         if (tmp == 1.0) {
  477           y = (rtNaN);
  478         } else if (tmp > 1.0) {
  479           if (u1 > 0.0) {
  480             y = (rtInf);
  481           } else {
  482             y = 0.0;
  483           }
  484         } else if (u1 > 0.0) {
  485           y = 0.0;
  486         } else {
  487           y = (rtInf);
  488         }
  489       } else if (tmp_0 == 0.0) {
  490         y = 1.0;
  491       } else if (tmp_0 == 1.0) {
  492         if (u1 > 0.0) {
  493           y = u0;
  494         } else {
  495           y = 1.0 / u0;
  496         }
  497       } else if (u1 == 2.0) {
  498         y = u0 * u0;
  499       } else if ((u1 == 0.5) && (u0 >= 0.0)) {
  500         y = sqrt(u0);
  501       } else if ((u0 < 0.0) && (u1 > floor(u1))) {
  502         y = (rtNaN);
  503       } else {
  504         y = pow(u0, u1);
  505       }
  506     }
  507   
  508     return y;
  509   }
  510   
  511   /* Initial conditions for atomic system: '<S14>/MATLAB Function' */
  512   void Controller_MATLABFunction_Init(void)
  513   {
  514     memset(&Controller_DWork.u_prev[0], 0, 12U * sizeof(real_T));
  515   }
  516   
  517   /* Output and update for atomic system: '<S14>/MATLAB Function' */
  518   void Controller_MATLABFunction(void)
  519   {
  520     real_T pow_sum;
  521     real_T w[12];
  522     real_T W[144];
  523     real_T b_1_T_pinv[12];
  524     real_T v[12];
  525     real_T Q[144];
  526     real_T gamma;
  527     real_T u_temp[12];
  528     real_T u[12];
  529     real_T F_2_hat[2];
  530     real_T v_0[12];
  531     real_T Q_0[144];
  532     real_T l;
  533     real_T u_0[12];
  534     real_T f_3_hat;
  535     int32_T i;
  536     real_T tmp[144];
  537     int32_T i_0;
  538     real_T tmp_0[12];
  539     real_T tmp_1[2];
  540     real_T b_1_T_pinv_0[144];
  541     real_T tmp_2[12];
  542     int32_T i_1;
  543   
  544     /* Constant: '<S14>/Constant3' */
  545     /* MATLAB Function 'Controll system/Thrust allocation/MATLAB Function': '<S127>:1' */
  546     /* % Step 1 */
  547     /*  setup variables and parameters */
  548     /* '<S127>:1:5' if isempty(u_prev) */
  549     /* '<S127>:1:10' if isempty(time_prev) */
  550     /* '<S127>:1:14' f_1 = tau(3); */
  551     /* '<S127>:1:15' b_1_T = T(3,:); */
  552     /* W_1 = eye(max(size(T))); */
  553     /*  Calculate weigths */
  554     /* '<S127>:1:19' pow_sum = sum(u_sat); */
  555     pow_sum = Controller_sum(Controller_P.Constant3_Value);
  556   
  557     /* '<S127>:1:20' w_1 = b_1_T; */
  558     for (i_0 = 0; i_0 < 12; i_0++) {
  559       /* Constant: '<S14>/Constant2' */
  560       w[i_0] = Controller_P.Constant2_Value_l[3 * i_0 + 2];
  561     }
  562   
  563     /* '<S127>:1:21' for i = 1:length(u_sat) */
  564     for (i = 0; i < 6; i++) {
  565       /* Constant: '<S14>/Constant3' */
  566       /* '<S127>:1:22' if u_sat(i) > 0 */
  567       if (Controller_P.Constant3_Value[i] > 0.0) {
  568         /* '<S127>:1:23' w_1(2*i) = pow_sum/u_sat(i); */
  569         w[((1 + i) << 1) - 1] = pow_sum / Controller_P.Constant3_Value[i];
  570   
  571         /* '<S127>:1:24' w_1(2*i-1) = pow_sum/u_sat(i); */
  572         w[((1 + i) << 1) - 2] = pow_sum / Controller_P.Constant3_Value[i];
  573       } else {
  574         /* '<S127>:1:25' else */
  575         /* '<S127>:1:26' w_1(2*i) = pow_sum; */
  576         w[((1 + i) << 1) - 1] = pow_sum;
  577   
  578         /* '<S127>:1:27' w_1(2*i-1) = pow_sum; */
  579         w[((1 + i) << 1) - 2] = pow_sum;
  580       }
  581     }
  582   
  583     /* '<S127>:1:30' W_1 = diag(w_1); */
  584     Controller_diag(w, W);
  585   
  586     /*  calculate u contribution */
  587     /* '<S127>:1:33' b_1_T_pinv = inv(W_1)*b_1_T'*inv(b_1_T*inv(W_1)*b_1_T'); */
  588     Controller_inv(W, tmp);
  589     pow_sum = 0.0;
  590     for (i_0 = 0; i_0 < 12; i_0++) {
  591       w[i_0] = 0.0;
  592       for (i = 0; i < 12; i++) {
  593         /* Constant: '<S14>/Constant2' */
  594         w[i_0] += Controller_P.Constant2_Value_l[3 * i + 2] * tmp[12 * i_0 + i];
  595       }
  596   
  597       /* Constant: '<S14>/Constant2' */
  598       pow_sum += Controller_P.Constant2_Value_l[3 * i_0 + 2] * w[i_0];
  599     }
  600   
  601     pow_sum = Controller_inv_i(pow_sum);
  602     Controller_inv(W, tmp);
  603   
  604     /* '<S127>:1:34' v_11 = b_1_T_pinv*f_1; */
  605     for (i_0 = 0; i_0 < 12; i_0++) {
  606       tmp_0[i_0] = 0.0;
  607       for (i = 0; i < 12; i++) {
  608         /* Constant: '<S14>/Constant2' */
  609         tmp_0[i_0] += tmp[12 * i + i_0] * Controller_P.Constant2_Value_l[3 * i + 2];
  610       }
  611   
  612       b_1_T_pinv[i_0] = tmp_0[i_0] * pow_sum;
  613       v[i_0] = b_1_T_pinv[i_0] * Controller_B.Product3[2];
  614     }
  615   
  616     /* '<S127>:1:35' Q_1 = eye(length(b_1_T)) - b_1_T_pinv*b_1_T; */
  617     Controller_eye(tmp);
  618     for (i_0 = 0; i_0 < 12; i_0++) {
  619       for (i = 0; i < 12; i++) {
  620         /* Constant: '<S14>/Constant2' */
  621         Q[i_0 + 12 * i] = tmp[12 * i + i_0] - Controller_P.Constant2_Value_l[3 * i
  622           + 2] * b_1_T_pinv[i_0];
  623       }
  624     }
  625   
  626     /* '<S127>:1:37' gamma_1 = 1; */
  627     gamma = 1.0;
  628   
  629     /* '<S127>:1:39' if sat_enable */
  630     if (Controller_U.TA_sat != 0.0) {
  631       for (i = 0; i < 6; i++) {
  632         /*  Determine scaling factor */
  633         /* '<S127>:1:41' u_cand = zeros(length(b_1_T)/2,1); */
  634         /* '<S127>:1:43' u_temp = v_11; */
  635         /* '<S127>:1:44' for i = 1:length(b_1_T)/2 */
  636         /* '<S127>:1:45' u_cand(i) = sqrt(u_temp(2*i-1)^2 + u_temp(2*i)^2); */
  637         f_3_hat = sqrt(rt_powd_snf(v[((1 + i) << 1) - 2], 2.0) + rt_powd_snf(v[((1
  638           + i) << 1) - 1], 2.0));
  639   
  640         /* Constant: '<S14>/Constant3' */
  641         /* '<S127>:1:46' if u_cand(i) > u_sat(i) */
  642         if ((f_3_hat > Controller_P.Constant3_Value[i]) && (gamma >
  643              Controller_P.Constant3_Value[i] / f_3_hat)) {
  644           /* '<S127>:1:47' if gamma_1 >  u_sat(i)/u_cand(i) */
  645           /* '<S127>:1:48' gamma_1 = u_sat(i)/u_cand(i); */
  646           gamma = Controller_P.Constant3_Value[i] / f_3_hat;
  647         }
  648       }
  649     }
  650   
  651     /* '<S127>:1:54' u_1 = gamma_1*v_11; */
  652     for (i = 0; i < 12; i++) {
  653       u[i] = gamma * v[i];
  654     }
  655   
  656     /* % Step 2 */
  657     /*  setup variables and parameters */
  658     /* '<S127>:1:58' B_2_T = T(1:2,:); */
  659     /* '<S127>:1:59' F_2_hat = [tau(1) tau(2)]' - gamma_1*B_2_T*v_11; */
  660     tmp_1[0] = Controller_B.Product3[0];
  661     tmp_1[1] = Controller_B.Product3[1];
  662     for (i_0 = 0; i_0 < 2; i_0++) {
  663       f_3_hat = 0.0;
  664       for (i = 0; i < 12; i++) {
  665         /* Constant: '<S14>/Constant2' */
  666         f_3_hat += Controller_P.Constant2_Value_l[3 * i + i_0] * gamma * v[i];
  667       }
  668   
  669       F_2_hat[i_0] = tmp_1[i_0] - f_3_hat;
  670     }
  671   
  672     /* '<S127>:1:60' f_2_hat = F_2_hat(1); */
  673     /* '<S127>:1:62' b_2_T = T(1,:); */
  674     /* '<S127>:1:63' W_2 = W_1; */
  675     /*  calculate u contribution */
  676     /* '<S127>:1:66' b_2_T_Q_1_pinv = inv(W_2)*Q_1'*b_2_T'*inv(b_2_T*Q_1*inv(W_2)*Q_1'*b_2_T'); */
  677     Controller_inv(W, tmp);
  678     for (i_0 = 0; i_0 < 12; i_0++) {
  679       tmp_0[i_0] = 0.0;
  680       for (i = 0; i < 12; i++) {
  681         /* Constant: '<S14>/Constant2' */
  682         tmp_0[i_0] += Q[12 * i_0 + i] * Controller_P.Constant2_Value_l[3 * i];
  683       }
  684     }
  685   
  686     for (i_0 = 0; i_0 < 12; i_0++) {
  687       u_temp[i_0] = 0.0;
  688       for (i = 0; i < 12; i++) {
  689         u_temp[i_0] += tmp[12 * i_0 + i] * tmp_0[i];
  690       }
  691     }
  692   
  693     pow_sum = 0.0;
  694     for (i_0 = 0; i_0 < 12; i_0++) {
  695       w[i_0] = 0.0;
  696       for (i = 0; i < 12; i++) {
  697         w[i_0] += Q[12 * i + i_0] * u_temp[i];
  698       }
  699   
  700       /* Constant: '<S14>/Constant2' */
  701       pow_sum += Controller_P.Constant2_Value_l[3 * i_0] * w[i_0];
  702     }
  703   
  704     pow_sum = Controller_inv_i(pow_sum);
  705     Controller_inv(W, tmp);
  706     for (i_0 = 0; i_0 < 12; i_0++) {
  707       for (i = 0; i < 12; i++) {
  708         b_1_T_pinv_0[i_0 + 12 * i] = 0.0;
  709         for (i_1 = 0; i_1 < 12; i_1++) {
  710           b_1_T_pinv_0[i_0 + 12 * i] = tmp[12 * i_1 + i_0] * Q[12 * i_1 + i] +
  711             b_1_T_pinv_0[12 * i + i_0];
  712         }
  713       }
  714     }
  715   
  716     /* '<S127>:1:67' v_21 = b_2_T_Q_1_pinv*f_2_hat; */
  717     for (i_0 = 0; i_0 < 12; i_0++) {
  718       tmp_0[i_0] = 0.0;
  719       for (i = 0; i < 12; i++) {
  720         /* Constant: '<S14>/Constant2' */
  721         tmp_0[i_0] += b_1_T_pinv_0[12 * i + i_0] * Controller_P.Constant2_Value_l
  722           [3 * i];
  723       }
  724   
  725       b_1_T_pinv[i_0] = tmp_0[i_0] * pow_sum;
  726       v_0[i_0] = b_1_T_pinv[i_0] * F_2_hat[0];
  727     }
  728   
  729     /* '<S127>:1:68' Q_2 = eye(length(b_1_T)) - b_2_T_Q_1_pinv*b_2_T*Q_1; */
  730     Controller_eye(tmp);
  731     for (i_0 = 0; i_0 < 12; i_0++) {
  732       for (i = 0; i < 12; i++) {
  733         /* Constant: '<S14>/Constant2' */
  734         b_1_T_pinv_0[i_0 + 12 * i] = Controller_P.Constant2_Value_l[3 * i] *
  735           b_1_T_pinv[i_0];
  736       }
  737     }
  738   
  739     for (i_0 = 0; i_0 < 12; i_0++) {
  740       for (i = 0; i < 12; i++) {
  741         f_3_hat = 0.0;
  742         for (i_1 = 0; i_1 < 12; i_1++) {
  743           f_3_hat += b_1_T_pinv_0[12 * i_1 + i_0] * Q[12 * i + i_1];
  744         }
  745   
  746         Q_0[i_0 + 12 * i] = tmp[12 * i + i_0] - f_3_hat;
  747       }
  748     }
  749   
  750     /* '<S127>:1:69' gamma_2 = 1; */
  751     pow_sum = 1.0;
  752   
  753     /* u_2 = Q_1*v_21; */
  754     /* '<S127>:1:73' if sat_enable */
  755     if (Controller_U.TA_sat != 0.0) {
  756       /*  Determine scaling factor */
  757       /* '<S127>:1:75' u_cand = zeros(length(b_1_T)/2,1); */
  758       /* '<S127>:1:77' u_temp = u_1 +  Q_1*v_21; */
  759       for (i_0 = 0; i_0 < 12; i_0++) {
  760         f_3_hat = 0.0;
  761         for (i = 0; i < 12; i++) {
  762           f_3_hat += Q[12 * i + i_0] * v_0[i];
  763         }
  764   
  765         u_temp[i_0] = u[i_0] + f_3_hat;
  766       }
  767   
  768       /* '<S127>:1:78' u_2_temp = Q_1*v_21; */
  769       for (i_0 = 0; i_0 < 12; i_0++) {
  770         b_1_T_pinv[i_0] = 0.0;
  771         for (i = 0; i < 12; i++) {
  772           b_1_T_pinv[i_0] += Q[12 * i + i_0] * v_0[i];
  773         }
  774       }
  775   
  776       /* '<S127>:1:79' for i = 1:length(b_1_T)/2 */
  777       for (i = 0; i < 6; i++) {
  778         /* Constant: '<S14>/Constant3' */
  779         /* '<S127>:1:80' u_cand(i) = sqrt(u_temp(2*i-1)^2 + u_temp(2*i)^2); */
  780         /* '<S127>:1:81' if u_cand(i) > u_sat(i) */
  781         if (sqrt(rt_powd_snf(u_temp[((1 + i) << 1) - 2], 2.0) + rt_powd_snf
  782                  (u_temp[((1 + i) << 1) - 1], 2.0)) >
  783             Controller_P.Constant3_Value[i]) {
  784           /* '<S127>:1:82' l_1 = u_sat(i) - sqrt(u_1(2*i)^2 + u_1(2*i-1)^2); */
  785           f_3_hat = Controller_P.Constant3_Value[i] - sqrt(rt_powd_snf(u[((1 + i) <<
  786             1) - 1], 2.0) + rt_powd_snf(u[((1 + i) << 1) - 2], 2.0));
  787   
  788           /* '<S127>:1:83' l_2 = sqrt(u_2_temp(2*i)^2 + u_2_temp(2*i-1)^2); */
  789           l = sqrt(rt_powd_snf(b_1_T_pinv[((1 + i) << 1) - 1], 2.0) + rt_powd_snf
  790                    (b_1_T_pinv[((1 + i) << 1) - 2], 2.0));
  791   
  792           /* '<S127>:1:85' if gamma_2 > l_1/l_2 */
  793           if (pow_sum > f_3_hat / l) {
  794             /* '<S127>:1:86' gamma_2 = l_1/l_2; */
  795             pow_sum = f_3_hat / l;
  796           }
  797         }
  798       }
  799     }
  800   
  801     /* '<S127>:1:91' u_2 = u_1 + gamma_2*Q_1*v_21; */
  802     for (i_0 = 0; i_0 < 12; i_0++) {
  803       f_3_hat = 0.0;
  804       for (i = 0; i < 12; i++) {
  805         f_3_hat += Q[12 * i + i_0] * pow_sum * v_0[i];
  806       }
  807   
  808       u_0[i_0] = u[i_0] + f_3_hat;
  809     }
  810   
  811     /* % Step 3 */
  812     /*  setup variables and parameters */
  813     /* '<S127>:1:95' b_3_T = T(2,:); */
  814     /* '<S127>:1:96' f_3_hat = tau(2) - gamma_1*b_3_T*v_11 - gamma_2*b_3_T*Q_1*v_21; */
  815     f_3_hat = 0.0;
  816     for (i_0 = 0; i_0 < 12; i_0++) {
  817       /* Constant: '<S14>/Constant2' */
  818       w[i_0] = Controller_P.Constant2_Value_l[3 * i_0 + 1] * gamma;
  819       f_3_hat += w[i_0] * v[i_0];
  820       w[i_0] = 0.0;
  821       for (i = 0; i < 12; i++) {
  822         /* Constant: '<S14>/Constant2' */
  823         w[i_0] += Controller_P.Constant2_Value_l[3 * i + 1] * pow_sum * Q[12 * i_0
  824           + i];
  825       }
  826     }
  827   
  828     pow_sum = 0.0;
  829     for (i = 0; i < 12; i++) {
  830       pow_sum += w[i] * v_0[i];
  831       tmp_2[i] = 0.0;
  832       for (i_0 = 0; i_0 < 12; i_0++) {
  833         /* Constant: '<S14>/Constant2' */
  834         tmp_2[i] += Controller_P.Constant2_Value_l[3 * i_0 + 1] * Q[12 * i + i_0];
  835       }
  836     }
  837   
  838     f_3_hat = (Controller_B.Product3[1] - f_3_hat) - pow_sum;
  839   
  840     /* '<S127>:1:97' W_3 = W_1; */
  841     /*  calculate u contribution */
  842     /* '<S127>:1:100' b_3_T_Q_1_Q_2_pinv = inv(W_3)*Q_2'*Q_1'*b_3_T'*inv(b_3_T*Q_1*Q_2*inv(W_3)*Q_2'*Q_1'*b_3_T'); */
  843     Controller_inv(W, tmp);
  844     for (i_0 = 0; i_0 < 12; i_0++) {
  845       tmp_0[i_0] = 0.0;
  846       for (i = 0; i < 12; i++) {
  847         tmp_0[i_0] += Q_0[12 * i_0 + i] * tmp_2[i];
  848       }
  849     }
  850   
  851     for (i_0 = 0; i_0 < 12; i_0++) {
  852       u_temp[i_0] = 0.0;
  853       for (i = 0; i < 12; i++) {
  854         u_temp[i_0] += tmp[12 * i_0 + i] * tmp_0[i];
  855       }
  856     }
  857   
  858     for (i_0 = 0; i_0 < 12; i_0++) {
  859       tmp_0[i_0] = 0.0;
  860       for (i = 0; i < 12; i++) {
  861         tmp_0[i_0] += Q_0[12 * i + i_0] * u_temp[i];
  862       }
  863     }
  864   
  865     pow_sum = 0.0;
  866     for (i_0 = 0; i_0 < 12; i_0++) {
  867       w[i_0] = 0.0;
  868       for (i = 0; i < 12; i++) {
  869         w[i_0] += Q[12 * i + i_0] * tmp_0[i];
  870       }
  871   
  872       /* Constant: '<S14>/Constant2' */
  873       pow_sum += Controller_P.Constant2_Value_l[3 * i_0 + 1] * w[i_0];
  874     }
  875   
  876     pow_sum = Controller_inv_i(pow_sum);
  877   
  878     /* '<S127>:1:101' v_31 = b_3_T_Q_1_Q_2_pinv*f_3_hat; */
  879     Controller_inv(W, tmp);
  880     for (i_0 = 0; i_0 < 12; i_0++) {
  881       for (i = 0; i < 12; i++) {
  882         b_1_T_pinv_0[i_0 + 12 * i] = 0.0;
  883         for (i_1 = 0; i_1 < 12; i_1++) {
  884           b_1_T_pinv_0[i_0 + 12 * i] = tmp[12 * i_1 + i_0] * Q_0[12 * i_1 + i] +
  885             b_1_T_pinv_0[12 * i + i_0];
  886         }
  887       }
  888     }
  889   
  890     for (i_0 = 0; i_0 < 12; i_0++) {
  891       for (i = 0; i < 12; i++) {
  892         tmp[i_0 + 12 * i] = 0.0;
  893         for (i_1 = 0; i_1 < 12; i_1++) {
  894           tmp[i_0 + 12 * i] = b_1_T_pinv_0[12 * i_1 + i_0] * Q[12 * i_1 + i] +
  895             tmp[12 * i + i_0];
  896         }
  897       }
  898     }
  899   
  900     for (i_0 = 0; i_0 < 12; i_0++) {
  901       tmp_0[i_0] = 0.0;
  902       for (i = 0; i < 12; i++) {
  903         /* Constant: '<S14>/Constant2' */
  904         tmp_0[i_0] += tmp[12 * i + i_0] * Controller_P.Constant2_Value_l[3 * i + 1];
  905       }
  906   
  907       u[i_0] = tmp_0[i_0] * pow_sum * f_3_hat;
  908     }
  909   
  910     /* '<S127>:1:102' gamma_3 = 1; */
  911     pow_sum = 1.0;
  912   
  913     /* '<S127>:1:104' if sat_enable */
  914     if (Controller_U.TA_sat != 0.0) {
  915       /*  Determine scaling factor */
  916       /* '<S127>:1:106' u_cand = zeros(length(b_1_T)/2,1); */
  917       /* '<S127>:1:108' u_temp = u_2 + Q_1*Q_2*v_31; */
  918       for (i_0 = 0; i_0 < 12; i_0++) {
  919         for (i = 0; i < 12; i++) {
  920           b_1_T_pinv_0[i_0 + 12 * i] = 0.0;
  921           for (i_1 = 0; i_1 < 12; i_1++) {
  922             b_1_T_pinv_0[i_0 + 12 * i] = Q[12 * i_1 + i_0] * Q_0[12 * i + i_1] +
  923               b_1_T_pinv_0[12 * i + i_0];
  924           }
  925         }
  926       }
  927   
  928       for (i_0 = 0; i_0 < 12; i_0++) {
  929         f_3_hat = 0.0;
  930         for (i = 0; i < 12; i++) {
  931           f_3_hat += b_1_T_pinv_0[12 * i + i_0] * u[i];
  932         }
  933   
  934         u_temp[i_0] = u_0[i_0] + f_3_hat;
  935       }
  936   
  937       /* '<S127>:1:109' u_3_temp = Q_1*Q_2*v_31; */
  938       for (i_0 = 0; i_0 < 12; i_0++) {
  939         for (i = 0; i < 12; i++) {
  940           b_1_T_pinv_0[i_0 + 12 * i] = 0.0;
  941           for (i_1 = 0; i_1 < 12; i_1++) {
  942             b_1_T_pinv_0[i_0 + 12 * i] = Q[12 * i_1 + i_0] * Q_0[12 * i + i_1] +
  943               b_1_T_pinv_0[12 * i + i_0];
  944           }
  945         }
  946       }
  947   
  948       for (i_0 = 0; i_0 < 12; i_0++) {
  949         b_1_T_pinv[i_0] = 0.0;
  950         for (i = 0; i < 12; i++) {
  951           b_1_T_pinv[i_0] += b_1_T_pinv_0[12 * i + i_0] * u[i];
  952         }
  953       }
  954   
  955       /* '<S127>:1:110' for i = 1:length(b_1_T)/2 */
  956       for (i = 0; i < 6; i++) {
  957         /* Constant: '<S14>/Constant3' */
  958         /* '<S127>:1:111' u_cand(i) = sqrt(u_temp(2*i-1)^2 + u_temp(2*i)^2); */
  959         /* '<S127>:1:112' if u_cand(i) > u_sat(i) */
  960         if (sqrt(rt_powd_snf(u_temp[((1 + i) << 1) - 2], 2.0) + rt_powd_snf
  961                  (u_temp[((1 + i) << 1) - 1], 2.0)) >
  962             Controller_P.Constant3_Value[i]) {
  963           /* '<S127>:1:113' l_1 = u_sat(i) - sqrt(u_2(2*i)^2 + u_2(2*i-1)^2); */
  964           f_3_hat = Controller_P.Constant3_Value[i] - sqrt(rt_powd_snf(u_0[((1 + i)
  965             << 1) - 1], 2.0) + rt_powd_snf(u_0[((1 + i) << 1) - 2], 2.0));
  966   
  967           /* '<S127>:1:114' l_2 = sqrt(u_3_temp(2*i)^2 + u_3_temp(2*i-1)^2); */
  968           l = sqrt(rt_powd_snf(b_1_T_pinv[((1 + i) << 1) - 1], 2.0) + rt_powd_snf
  969                    (b_1_T_pinv[((1 + i) << 1) - 2], 2.0));
  970   
  971           /* '<S127>:1:116' if gamma_3 > l_1/l_2 */
  972           if (pow_sum > f_3_hat / l) {
  973             /* '<S127>:1:117' gamma_3 = l_1/l_2; */
  974             pow_sum = f_3_hat / l;
  975           }
  976         }
  977       }
  978     }
  979   
  980     /* '<S127>:1:123' u_3 = u_2 + gamma_3*Q_1*Q_2*v_31; */
  981     for (i_0 = 0; i_0 < 12; i_0++) {
  982       for (i = 0; i < 12; i++) {
  983         b_1_T_pinv_0[i_0 + 12 * i] = 0.0;
  984         for (i_1 = 0; i_1 < 12; i_1++) {
  985           b_1_T_pinv_0[i_0 + 12 * i] = Q[12 * i_1 + i_0] * pow_sum * Q_0[12 * i +
  986             i_1] + b_1_T_pinv_0[12 * i + i_0];
  987         }
  988       }
  989     }
  990   
  991     for (i_0 = 0; i_0 < 12; i_0++) {
  992       f_3_hat = 0.0;
  993       for (i = 0; i < 12; i++) {
  994         f_3_hat += b_1_T_pinv_0[12 * i + i_0] * u[i];
  995       }
  996   
  997       Controller_B.u[i_0] = u_0[i_0] + f_3_hat;
  998     }
  999   
 1000     /* '<S127>:1:125' u = u_3; */
 1001     /* '<S127>:1:127' if rate_enable */
 1002     if (Controller_U.TA_rate_sat != 0.0) {
 1003       /*  Calculate rate vector */
 1004       /* '<S127>:1:129' rate = (u-u_prev)/0.01; */
 1005       for (i_0 = 0; i_0 < 12; i_0++) {
 1006         Controller_B.u[i_0] = (Controller_B.u[i_0] - Controller_DWork.u_prev[i_0])
 1007           / 0.01;
 1008       }
 1009   
 1010       /* '<S127>:1:130' rate_cand = zeros(length(b_1_T)/2,1); */
 1011       /* '<S127>:1:131' gamma_rate = 1; */
 1012       pow_sum = 1.0;
 1013   
 1014       /* '<S127>:1:133' for i = 1:length(b_1_T)/2 */
 1015       for (i = 0; i < 6; i++) {
 1016         /* '<S127>:1:134' rate_cand(i) = sqrt(rate(2*i-1)^2 + rate(2*i)^2); */
 1017         f_3_hat = sqrt(rt_powd_snf(Controller_B.u[((1 + i) << 1) - 2], 2.0) +
 1018                        rt_powd_snf(Controller_B.u[((1 + i) << 1) - 1], 2.0));
 1019   
 1020         /* Constant: '<S14>/Constant8' */
 1021         /* '<S127>:1:135' if rate_cand(i) > rate_sat(i) */
 1022         if ((f_3_hat > Controller_P.Constant8_Value[i]) && (pow_sum >
 1023              Controller_P.Constant8_Value[i] / f_3_hat)) {
 1024           /* '<S127>:1:136' if gamma_rate >  rate_sat(i)/(rate_cand(i)) */
 1025           /* '<S127>:1:137' gamma_rate = rate_sat(i)/(rate_cand(i)); */
 1026           pow_sum = Controller_P.Constant8_Value[i] / f_3_hat;
 1027         }
 1028   
 1029         /* End of Constant: '<S14>/Constant8' */
 1030       }
 1031   
 1032       /* '<S127>:1:142' u = u_prev + 0.01*gamma_rate*rate; */
 1033       pow_sum *= 0.01;
 1034   
 1035       /* '<S127>:1:143' u_prev = u; */
 1036       for (i_0 = 0; i_0 < 12; i_0++) {
 1037         Controller_B.u[i_0] = pow_sum * Controller_B.u[i_0] +
 1038           Controller_DWork.u_prev[i_0];
 1039         Controller_DWork.u_prev[i_0] = Controller_B.u[i_0];
 1040       }
 1041     }
 1042   
 1043     /* tau_out = T*u; */
 1044   }
 1045   
 1046   real_T rt_urand_Upu32_Yd_f_pw_snf(uint32_T *u)
 1047   {
 1048     uint32_T lo;
 1049     uint32_T hi;
 1050   
 1051     /* Uniform random number generator (random number between 0 and 1)
 1052   
 1053        #define IA      16807                      magic multiplier = 7^5
 1054        #define IM      2147483647                 modulus = 2^31-1
 1055        #define IQ      127773                     IM div IA
 1056        #define IR      2836                       IM modulo IA
 1057        #define S       4.656612875245797e-10      reciprocal of 2^31-1
 1058        test = IA * (seed % IQ) - IR * (seed/IQ)
 1059        seed = test < 0 ? (test + IM) : test
 1060        return (seed*S)
 1061      */
 1062     lo = *u % 127773U * 16807U;
 1063     hi = *u / 127773U * 2836U;
 1064     if (lo < hi) {
 1065       *u = 2147483647U - (hi - lo);
 1066     } else {
 1067       *u = lo - hi;
 1068     }
 1069   
 1070     return (real_T)*u * 4.6566128752457969E-10;
 1071   }
 1072   
 1073   real_T rt_nrand_Upu32_Yd_f_pw_snf(uint32_T *u)
 1074   {
 1075     real_T sr;
 1076     real_T si;
 1077   
 1078     /* Normal (Gaussian) random number generator */
 1079     do {
 1080       sr = 2.0 * rt_urand_Upu32_Yd_f_pw_snf(u) - 1.0;
 1081       si = 2.0 * rt_urand_Upu32_Yd_f_pw_snf(u) - 1.0;
 1082       si = sr * sr + si * si;
 1083     } while (si > 1.0);
 1084   
 1085     return sqrt(-2.0 * log(si) / si) * sr;
 1086   }
 1087   
 1088   real_T rt_atan2d_snf(real_T u0, real_T u1)
 1089   {
 1090     real_T y;
 1091     int32_T u;
 1092     int32_T u_0;
 1093     if (rtIsNaN(u0) || rtIsNaN(u1)) {
 1094       y = (rtNaN);
 1095     } else if (rtIsInf(u0) && rtIsInf(u1)) {
 1096       if (u0 > 0.0) {
 1097         u = 1;
 1098       } else {
 1099         u = -1;
 1100       }
 1101   
 1102       if (u1 > 0.0) {
 1103         u_0 = 1;
 1104       } else {
 1105         u_0 = -1;
 1106       }
 1107   
 1108       y = atan2((real_T)u, (real_T)u_0);
 1109     } else if (u1 == 0.0) {
 1110       if (u0 > 0.0) {
 1111         y = RT_PI / 2.0;
 1112       } else if (u0 < 0.0) {
 1113         y = -(RT_PI / 2.0);
 1114       } else {
 1115         y = 0.0;
 1116       }
 1117     } else {
 1118       y = atan2(u0, u1);
 1119     }
 1120   
 1121     return y;
 1122   }
 1123   
 1124   /* Function for MATLAB Function: '<S16>/Control law parameter calculation' */
 1125   static void Controller_conv(const real_T A[2], const real_T B[3], real_T C[4])
 1126   {
 1127     real_T s;
 1128     int32_T k;
 1129     s = 0.0;
 1130     for (k = 0; k + 1 < 2; k++) {
 1131       s += A[k] * B[-k];
 1132     }
 1133   
 1134     C[0] = s;
 1135     s = 0.0;
 1136     for (k = 0; k + 1 < 3; k++) {
 1137       s += B[1 - k] * A[k];
 1138     }
 1139   
 1140     C[1] = s;
 1141     s = 0.0;
 1142     for (k = 0; k + 1 < 3; k++) {
 1143       s += B[2 - k] * A[k];
 1144     }
 1145   
 1146     C[2] = s;
 1147     s = 0.0;
 1148     for (k = 1; k + 1 < 3; k++) {
 1149       s += B[3 - k] * A[k];
 1150     }
 1151   
 1152     C[3] = s;
 1153   }
 1154   
 1155   /* Model step function */
 1156   void Controller_step(void)
 1157   {
 1158     /* local block i/o variables */
 1159     real_T rtb_TransferFcn2;
 1160     real_T rtb_TransferFcn1;
 1161     real_T rtb_TransferFcn;
 1162     real_T rtb_TransferFcn_j;
 1163     real_T rtb_TransferFcn1_n;
 1164     real_T rtb_TransferFcn2_b;
 1165     real_T rtb_TransferFcn3;
 1166     real_T rtb_TransferFcn4;
 1167     real_T rtb_TransferFcn5;
 1168     real_T rtb_Clock;
 1169     real_T rtb_Clock_m;
 1170     real_T rtb_Clock_f;
 1171     real_T rtb_Gain_i[3];
 1172     real_T rtb_TransferFcn2_h;
 1173     real_T rtb_TransferFcn1_o;
 1174     real_T rtb_Gain5_g;
 1175     real_T kp[9];
 1176     real_T p[9];
 1177     real_T p_0[9];
 1178     real_T diophantine_eq[4];
 1179     real_T q[2];
 1180     static const int16_T M_0deu[3] = { 10000, 10000, 12000 };
 1181   
 1182     static const int16_T b_a[4] = { 700, 0, 0, 200 };
 1183   
 1184     int32_T k;
 1185     static const int16_T M_0du[3] = { 10000, 10000, 12000 };
 1186   
 1187     static const int16_T a[4] = { 6100, 0, 0, 200 };
 1188   
 1189     real_T rtb_Gain1_f;
 1190     real_T rtb_D[3];
 1191     real_T rtb_VectorConcatenate_d[9];
 1192     real_T rtb_Integrator_d[3];
 1193     real_T rtb_Sum_be[3];
 1194     real_T rtb_Row1;
 1195     real_T rtb_Row2;
 1196     real_T rtb_Row3;
 1197     real_T rtb_VectorConcatenate_p[9];
 1198     real_T rtb_Gain_pk[3];
 1199     real_T rtb_VectorConcatenate_c[9];
 1200     real_T rtb_Sum_k;
 1201     real_T rtb_Sum2_i;
 1202     real_T rtb_Integrator1_i;
 1203     real_T rtb_VectorConcatenate_h[9];
 1204     real_T rtb_Step;
 1205     int32_T idx2;
 1206     real_T rtb_Divide;
 1207     real_T rtb_Gain_ia;
 1208     real_T rtb_VectorConcatenate_n[9];
 1209     real_T rtb_VectorConcatenate_ck[9];
 1210     real_T rtb_VectorConcatenate_k[9];
 1211     real_T rtb_VectorConcatenate_nu[9];
 1212     real_T rtb_VectorConcatenate_kf[9];
 1213     real_T rtb_sincos_o2[3];
 1214     real_T rtb_VectorConcatenate_nt[9];
 1215     real_T rtb_Gain6[3];
 1216     real_T rtb_Sum_g[3];
 1217     real_T rtb_ypm[3];
 1218     real_T rtb_yFm[3];
 1219     real_T rtb_VectorConcatenate_g[9];
 1220     real_T rtb_VectorConcatenate_pj[9];
 1221     real_T rtb_VectorConcatenate_a[9];
 1222     real_T rtb_VectorConcatenate_l[9];
 1223     real_T rtb_Product_g[3];
 1224     real_T rtb_VectorConcatenate_o[9];
 1225     real_T rtb_VectorConcatenate_kr[9];
 1226     real_T rtb_VectorConcatenate_b[9];
 1227     real_T rtb_VectorConcatenate_ou[9];
 1228     real_T rtb_TmpSignalConversionAtSFun_g[3];
 1229     real_T rtb_TmpSignalConversionAtSFu_gy[3];
 1230     real_T rtb_TmpSignalConversionAtSFun_d[3];
 1231     real_T rtb_TmpSignalConversionAtSFun_n[3];
 1232     real_T rtb_Sum3_g;
 1233     real_T rtb_Z_m[3];
 1234     real_T rtb_k_m_r[3];
 1235     real_T rtb_Integrator2[6];
 1236     real_T rtb_Integrator6[6];
 1237     real_T rtb_Integrator10[18];
 1238     real_T rtb_Integrator3[18];
 1239     real_T rtb_theta_ctrl_hat[12];
 1240     real_T rtb_R_m[9];
 1241     real_T rtb_frc_ctrl[3];
 1242     real_T rtb_mrac_theta_p[36];
 1243     int32_T i;
 1244     real_T tmp[2];
 1245     real_T tmp_0[4];
 1246     real_T tmp_1[4];
 1247     real_T tmp_2[3];
 1248     real_T rtb_VectorConcatenate_p_0[3];
 1249     real_T rtb_VectorConcatenate_p_1[3];
 1250     real_T rtb_Sum1_pd_0[12];
 1251     real_T tmp_3[9];
 1252     real_T tmp_4[9];
 1253     real_T a_data[2];
 1254     real_T rtb_Normalization_data[2];
 1255     real_T rtb_Integrator_idx;
 1256     real_T rtb_Integrator_idx_0;
 1257     real_T rtb_Integrator_idx_1;
 1258     real_T rtb_Integrator2_n_idx;
 1259     real_T rtb_Integrator2_n_idx_0;
 1260     real_T rtb_Integrator2_n_idx_1;
 1261     real_T rtb_sincos_o1_e_idx;
 1262     real_T rtb_Sum5_o_idx;
 1263     real_T rtb_Sum5_o_idx_0;
 1264     real_T rtb_Sum5_o_idx_1;
 1265     real_T rtb_Sum2_a_idx;
 1266     real_T rtb_Sum2_a_idx_0;
 1267     real_T rtb_Sum2_a_idx_1;
 1268     real_T rtb_sincos_o1_idx;
 1269     real_T rtb_sincos_o1_idx_0;
 1270     real_T rtb_sincos_o1_idx_1;
 1271     real_T rtb_Sum1_pd_idx;
 1272     real_T rtb_Sum3_c_idx;
 1273     real_T rtb_Sum3_c_idx_0;
 1274     real_T rtb_Sum3_c_idx_1;
 1275     real_T rtb_Gain1_l_idx;
 1276     real_T rtb_Gain1_l_idx_0;
 1277     real_T rtb_Gain1_l_idx_1;
 1278     real_T rtb_Normalization_idx;
 1279     real_T rtb_Normalization_idx_0;
 1280     real_T omega_idx;
 1281     if (rtmIsMajorTimeStep(Controller_M)) {
 1282       /* set solver stop time */
 1283       if (!(Controller_M->Timing.clockTick0+1)) {
 1284         rtsiSetSolverStopTime(&Controller_M->solverInfo,
 1285                               ((Controller_M->Timing.clockTickH0 + 1) *
 1286           Controller_M->Timing.stepSize0 * 4294967296.0));
 1287       } else {
 1288         rtsiSetSolverStopTime(&Controller_M->solverInfo,
 1289                               ((Controller_M->Timing.clockTick0 + 1) *
 1290           Controller_M->Timing.stepSize0 + Controller_M->Timing.clockTickH0 *
 1291           Controller_M->Timing.stepSize0 * 4294967296.0));
 1292       }
 1293     }                                    /* end MajorTimeStep */
 1294   
 1295     /* Update absolute time of base rate at minor time step */
 1296     if (rtmIsMinorTimeStep(Controller_M)) {
 1297       Controller_M->Timing.t[0] = rtsiGetT(&Controller_M->solverInfo);
 1298     }
 1299   
 1300     /* Integrator: '<S14>/Integrator' */
 1301     rtb_Integrator_idx_1 = Controller_X.Integrator_CSTATE[0];
 1302     rtb_Integrator_idx_0 = Controller_X.Integrator_CSTATE[1];
 1303     rtb_Integrator_idx = Controller_X.Integrator_CSTATE[2];
 1304   
 1305     /* Outport: '<Root>/tau_X' incorporates:
 1306      *  Integrator: '<S14>/Integrator'
 1307      */
 1308     Controller_Y.tau_X = Controller_X.Integrator_CSTATE[0];
 1309   
 1310     /* Outport: '<Root>/tau_Y' incorporates:
 1311      *  Integrator: '<S14>/Integrator'
 1312      */
 1313     Controller_Y.tau_Y = Controller_X.Integrator_CSTATE[1];
 1314   
 1315     /* Outport: '<Root>/tau_N' incorporates:
 1316      *  Integrator: '<S14>/Integrator'
 1317      */
 1318     Controller_Y.tau_N = Controller_X.Integrator_CSTATE[2];
 1319   
 1320     /* Integrator: '<S104>/Integrator1' */
 1321     rtb_Sum2_a_idx_1 = Controller_X.Integrator1_CSTATE[0];
 1322     rtb_Sum2_a_idx_0 = Controller_X.Integrator1_CSTATE[1];
 1323     rtb_Sum2_a_idx = Controller_X.Integrator1_CSTATE[2];
 1324   
 1325     /* TransferFcn: '<S77>/Transfer Fcn2' */
 1326     rtb_TransferFcn2 = Controller_P.TransferFcn2_C[0]*
 1327       Controller_X.TransferFcn2_CSTATE[0]
 1328       + Controller_P.TransferFcn2_C[1]*Controller_X.TransferFcn2_CSTATE[1];
 1329   
 1330     /* Step: '<S8>/Step2' */
 1331     if (Controller_M->Timing.t[0] < Controller_P.Step2_Time) {
 1332       rtb_Normalization_idx = Controller_P.Step2_Y0;
 1333     } else {
 1334       rtb_Normalization_idx = Controller_P.Step2_YFinal;
 1335     }
 1336   
 1337     /* End of Step: '<S8>/Step2' */
 1338   
 1339     /* Step: '<S8>/Step3' */
 1340     if (Controller_M->Timing.t[0] < Controller_P.Step3_Time) {
 1341       rtb_Normalization_idx_0 = Controller_P.Step3_Y0;
 1342     } else {
 1343       rtb_Normalization_idx_0 = Controller_P.Step3_YFinal;
 1344     }
 1345   
 1346     /* End of Step: '<S8>/Step3' */
 1347   
 1348     /* Sum: '<S8>/Sum' incorporates:
 1349      *  Gain: '<S8>/Gain'
 1350      *  Inport: '<Root>/eta_d_y'
 1351      *  Sum: '<S8>/Sum3'
 1352      */
 1353     rtb_Gain_ia = (rtb_Normalization_idx + rtb_Normalization_idx_0) *
 1354       Controller_P.Gain_Gain + Controller_U.eta_d_y;
 1355   
 1356     /* Step: '<S8>/Step' */
 1357     if (Controller_M->Timing.t[0] < Controller_P.Step_Time) {
 1358       rtb_Normalization_idx = Controller_P.Step_Y0;
 1359     } else {
 1360       rtb_Normalization_idx = Controller_P.Step_YFinal;
 1361     }
 1362   
 1363     /* End of Step: '<S8>/Step' */
 1364   
 1365     /* Step: '<S8>/Step1' */
 1366     if (Controller_M->Timing.t[0] < Controller_P.Step1_Time) {
 1367       rtb_Normalization_idx_0 = Controller_P.Step1_Y0;
 1368     } else {
 1369       rtb_Normalization_idx_0 = Controller_P.Step1_YFinal;
 1370     }
 1371   
 1372     /* End of Step: '<S8>/Step1' */
 1373   
 1374     /* Gain: '<S135>/Gain1' incorporates:
 1375      *  Gain: '<S8>/Gain1'
 1376      *  Inport: '<Root>/eta_d_psi'
 1377      *  Sum: '<S8>/Sum1'
 1378      *  Sum: '<S8>/Sum2'
 1379      */
 1380     rtb_Gain1_f = ((rtb_Normalization_idx + rtb_Normalization_idx_0) *
 1381                    Controller_P.Gain1_Gain + Controller_U.eta_d_psi) *
 1382       Controller_P.Gain1_Gain_j;
 1383   
 1384     /* Sum: '<S15>/Sum' incorporates:
 1385      *  Inport: '<Root>/eta_d_x'
 1386      */
 1387     rtb_D[0] = Controller_U.eta_d_x - rtb_Sum2_a_idx_1;
 1388     rtb_D[1] = rtb_Gain_ia - rtb_Sum2_a_idx_0;
 1389     rtb_D[2] = rtb_Gain1_f - rtb_TransferFcn2;
 1390   
 1391     /* Outport: '<Root>/eta_err_N' */
 1392     Controller_Y.eta_err_N = rtb_D[0];
 1393   
 1394     /* Outport: '<Root>/eta_err_E' */
 1395     Controller_Y.eta_err_E = rtb_D[1];
 1396   
 1397     /* Outport: '<Root>/eta_err_psi' */
 1398     Controller_Y.eta_err_psi = rtb_D[2];
 1399   
 1400     /* Outport: '<Root>/eta_e_N_AFF' */
 1401     Controller_Y.eta_e_N_AFF = rtb_Sum2_a_idx_1;
 1402   
 1403     /* Outport: '<Root>/eta_e_E_AFF' */
 1404     Controller_Y.eta_e_E_AFF = rtb_Sum2_a_idx_0;
 1405   
 1406     /* Outport: '<Root>/eta_e_psi_AFF' */
 1407     Controller_Y.eta_e_psi_AFF = rtb_TransferFcn2;
 1408   
 1409     /* TransferFcn: '<S77>/Transfer Fcn1' */
 1410     rtb_TransferFcn1 = Controller_P.TransferFcn1_C[0]*
 1411       Controller_X.TransferFcn1_CSTATE[0]
 1412       + Controller_P.TransferFcn1_C[1]*Controller_X.TransferFcn1_CSTATE[1];
 1413   
 1414     /* TransferFcn: '<S77>/Transfer Fcn' */
 1415     rtb_TransferFcn = Controller_P.TransferFcn_C[0]*
 1416       Controller_X.TransferFcn_CSTATE[0]
 1417       + Controller_P.TransferFcn_C[1]*Controller_X.TransferFcn_CSTATE[1];
 1418   
 1419     /* SignalConversion: '<S109>/TmpSignal ConversionAtsincosInport1' */
 1420     rtb_sincos_o1_idx_1 = rtb_TransferFcn2;
 1421     rtb_sincos_o1_idx_0 = rtb_TransferFcn1;
 1422     rtb_sincos_o1_idx = rtb_TransferFcn;
 1423   
 1424     /* Trigonometry: '<S114>/sincos' incorporates:
 1425      *  SignalConversion: '<S109>/TmpSignal ConversionAtsincosInport1'
 1426      */
 1427     rtb_Sum_be[0] = sin(rtb_TransferFcn2);
 1428     rtb_Integrator_d[0] = cos(rtb_TransferFcn2);
 1429     rtb_Sum_be[1] = sin(rtb_TransferFcn1);
 1430     rtb_Integrator_d[1] = cos(rtb_TransferFcn1);
 1431     rtb_Sum_be[2] = sin(rtb_TransferFcn);
 1432     rtb_Integrator_d[2] = cos(rtb_TransferFcn);
 1433   
 1434     /* Fcn: '<S114>/Fcn11' */
 1435     rtb_VectorConcatenate_d[0] = rtb_Integrator_d[1] * rtb_Integrator_d[0];
 1436   
 1437     /* Fcn: '<S114>/Fcn21' */
 1438     rtb_VectorConcatenate_d[1] = rtb_Sum_be[2] * rtb_Sum_be[1] * rtb_Integrator_d
 1439       [0] - rtb_Integrator_d[2] * rtb_Sum_be[0];
 1440   
 1441     /* Fcn: '<S114>/Fcn31' */
 1442     rtb_VectorConcatenate_d[2] = rtb_Integrator_d[2] * rtb_Sum_be[1] *
 1443       rtb_Integrator_d[0] + rtb_Sum_be[2] * rtb_Sum_be[0];
 1444   
 1445     /* Fcn: '<S114>/Fcn12' */
 1446     rtb_VectorConcatenate_d[3] = rtb_Integrator_d[1] * rtb_Sum_be[0];
 1447   
 1448     /* Fcn: '<S114>/Fcn22' */
 1449     rtb_VectorConcatenate_d[4] = rtb_Sum_be[2] * rtb_Sum_be[1] * rtb_Sum_be[0] +
 1450       rtb_Integrator_d[2] * rtb_Integrator_d[0];
 1451   
 1452     /* Fcn: '<S114>/Fcn32' */
 1453     rtb_VectorConcatenate_d[5] = rtb_Integrator_d[2] * rtb_Sum_be[1] * rtb_Sum_be
 1454       [0] - rtb_Sum_be[2] * rtb_Integrator_d[0];
 1455   
 1456     /* Fcn: '<S114>/Fcn13' */
 1457     rtb_VectorConcatenate_d[6] = -rtb_Sum_be[1];
 1458   
 1459     /* Fcn: '<S114>/Fcn23' */
 1460     rtb_VectorConcatenate_d[7] = rtb_Sum_be[2] * rtb_Integrator_d[1];
 1461   
 1462     /* Fcn: '<S114>/Fcn33' */
 1463     rtb_VectorConcatenate_d[8] = rtb_Integrator_d[2] * rtb_Integrator_d[1];
 1464   
 1465     /* Integrator: '<S104>/Integrator' */
 1466     rtb_Integrator_d[0] = Controller_X.Integrator_CSTATE_g[0];
 1467     rtb_Integrator_d[1] = Controller_X.Integrator_CSTATE_g[1];
 1468     rtb_Integrator_d[2] = Controller_X.Integrator_CSTATE_g[2];
 1469   
 1470     /* Product: '<S107>/Product1' incorporates:
 1471      *  Integrator: '<S104>/Integrator'
 1472      */
 1473     for (k = 0; k < 3; k++) {
 1474       rtb_Sum_be[k] = rtb_VectorConcatenate_d[k + 6] *
 1475         Controller_X.Integrator_CSTATE_g[2] + (rtb_VectorConcatenate_d[k + 3] *
 1476         Controller_X.Integrator_CSTATE_g[1] + rtb_VectorConcatenate_d[k] *
 1477         Controller_X.Integrator_CSTATE_g[0]);
 1478     }
 1479   
 1480     /* End of Product: '<S107>/Product1' */
 1481   
 1482     /* Outport: '<Root>/nu_e_u_AFF' */
 1483     Controller_Y.nu_e_u_AFF = rtb_Sum_be[0];
 1484   
 1485     /* Outport: '<Root>/nu_e_v_AFF' */
 1486     Controller_Y.nu_e_v_AFF = rtb_Sum_be[1];
 1487   
 1488     /* Integrator: '<S106>/Integrator' */
 1489     Controller_B.Integrator[0] = Controller_X.Integrator_CSTATE_f[0];
 1490     Controller_B.Integrator[1] = Controller_X.Integrator_CSTATE_f[1];
 1491     Controller_B.Integrator[2] = Controller_X.Integrator_CSTATE_f[2];
 1492   
 1493     /* Outport: '<Root>/nu_e_r_AFF' */
 1494     Controller_Y.nu_e_r_AFF = Controller_B.Integrator[2];
 1495   
 1496     /* Integrator: '<S107>/Integrator2' */
 1497     rtb_Sum5_o_idx_1 = Controller_X.Integrator2_CSTATE[0];
 1498     rtb_Sum5_o_idx_0 = Controller_X.Integrator2_CSTATE[1];
 1499     rtb_Sum5_o_idx = Controller_X.Integrator2_CSTATE[2];
 1500   
 1501     /* Gain: '<S107>/Gain' incorporates:
 1502      *  Integrator: '<S107>/Integrator2'
 1503      */
 1504     Controller_B.Gain[0] = Controller_P.Gain_Gain_a *
 1505       Controller_X.Integrator2_CSTATE[0];
 1506     Controller_B.Gain[1] = Controller_P.Gain_Gain_a *
 1507       Controller_X.Integrator2_CSTATE[1];
 1508     Controller_B.Gain[2] = Controller_P.Gain_Gain_a *
 1509       Controller_X.Integrator2_CSTATE[2];
 1510   
 1511     /* Outport: '<Root>/d_e_x_AFF' */
 1512     Controller_Y.d_e_x_AFF = Controller_B.Gain[0];
 1513   
 1514     /* Outport: '<Root>/d_e_y_AFF' */
 1515     Controller_Y.d_e_y_AFF = Controller_B.Gain[1];
 1516   
 1517     /* Outport: '<Root>/d_e_psi_AFF' */
 1518     Controller_Y.d_e_psi_AFF = Controller_B.Gain[2];
 1519     for (i = 0; i < 6; i++) {
 1520       /* Integrator: '<S16>/Integrator2' */
 1521       rtb_Integrator2[i] = Controller_X.Integrator2_CSTATE_f[i];
 1522   
 1523       /* Integrator: '<S16>/Integrator6' */
 1524       rtb_Integrator6[i] = Controller_X.Integrator6_CSTATE[i];
 1525     }
 1526   
 1527     /* Fcn: '<S27>/Row1' incorporates:
 1528      *  Inport: '<Root>/eta_d_x'
 1529      */
 1530     Controller_B.Row1 = cos(rtb_TransferFcn2) * Controller_U.eta_d_x + sin
 1531       (rtb_TransferFcn2) * rtb_Gain_ia;
 1532   
 1533     /* Fcn: '<S27>/Row2' incorporates:
 1534      *  Inport: '<Root>/eta_d_x'
 1535      */
 1536     Controller_B.Row2 = -sin(rtb_TransferFcn2) * Controller_U.eta_d_x + cos
 1537       (rtb_TransferFcn2) * rtb_Gain_ia;
 1538   
 1539     /* Fcn: '<S27>/Row3' */
 1540     Controller_B.Row3 = rtb_Gain1_f;
 1541   
 1542     /* TransferFcn: '<S22>/Transfer Fcn' */
 1543     rtb_TransferFcn_j = Controller_P.TransferFcn_C_d*
 1544       Controller_X.TransferFcn_CSTATE_e;
 1545   
 1546     /* TransferFcn: '<S22>/Transfer Fcn1' */
 1547     rtb_TransferFcn1_n = Controller_P.TransferFcn1_C_o*
 1548       Controller_X.TransferFcn1_CSTATE_d;
 1549   
 1550     /* TransferFcn: '<S22>/Transfer Fcn2' */
 1551     rtb_TransferFcn2_b = Controller_P.TransferFcn2_C_n*
 1552       Controller_X.TransferFcn2_CSTATE_b;
 1553   
 1554     /* TransferFcn: '<S22>/Transfer Fcn3' */
 1555     rtb_TransferFcn3 = Controller_P.TransferFcn3_D*Controller_B.Gain[0];
 1556   
 1557     /* TransferFcn: '<S22>/Transfer Fcn4' */
 1558     rtb_TransferFcn4 = Controller_P.TransferFcn4_D*Controller_B.Gain[1];
 1559   
 1560     /* TransferFcn: '<S22>/Transfer Fcn5' */
 1561     rtb_TransferFcn5 = Controller_P.TransferFcn5_D*Controller_B.Gain[2];
 1562   
 1563     /* Fcn: '<S26>/Row1' */
 1564     rtb_Row1 = cos(rtb_TransferFcn2) * rtb_Sum2_a_idx_1 + sin(rtb_TransferFcn2) *
 1565       rtb_Sum2_a_idx_0;
 1566   
 1567     /* Fcn: '<S26>/Row2' */
 1568     rtb_Row2 = -sin(rtb_TransferFcn2) * rtb_Sum2_a_idx_1 + cos(rtb_TransferFcn2) *
 1569       rtb_Sum2_a_idx_0;
 1570   
 1571     /* Fcn: '<S26>/Row3' */
 1572     rtb_Row3 = rtb_TransferFcn2;
 1573   
 1574     /* SignalConversion: '<S31>/TmpSignal ConversionAt SFunction Inport3' incorporates:
 1575      *  MATLAB Function: '<S16>/reference_model'
 1576      */
 1577     rtb_TmpSignalConversionAtSFun_g[0] = Controller_B.Row1;
 1578     rtb_TmpSignalConversionAtSFun_g[1] = Controller_B.Row2;
 1579     rtb_TmpSignalConversionAtSFun_g[2] = Controller_B.Row3;
 1580   
 1581     /* SignalConversion: '<S31>/TmpSignal ConversionAt SFunction Inport4' incorporates:
 1582      *  MATLAB Function: '<S16>/reference_model'
 1583      */
 1584     rtb_TmpSignalConversionAtSFu_gy[0] = rtb_TransferFcn_j;
 1585     rtb_TmpSignalConversionAtSFu_gy[1] = rtb_TransferFcn1_n;
 1586     rtb_TmpSignalConversionAtSFu_gy[2] = rtb_TransferFcn2_b;
 1587   
 1588     /* SignalConversion: '<S31>/TmpSignal ConversionAt SFunction Inport8' incorporates:
 1589      *  MATLAB Function: '<S16>/reference_model'
 1590      */
 1591     rtb_TmpSignalConversionAtSFun_d[0] = rtb_TransferFcn3;
 1592     rtb_TmpSignalConversionAtSFun_d[1] = rtb_TransferFcn4;
 1593     rtb_TmpSignalConversionAtSFun_d[2] = rtb_TransferFcn5;
 1594   
 1595     /* SignalConversion: '<S31>/TmpSignal ConversionAt SFunction Inport9' incorporates:
 1596      *  Fcn: '<S26>/Row3'
 1597      *  MATLAB Function: '<S16>/reference_model'
 1598      */
 1599     rtb_TmpSignalConversionAtSFun_n[0] = rtb_Row1;
 1600     rtb_TmpSignalConversionAtSFun_n[1] = rtb_Row2;
 1601     rtb_TmpSignalConversionAtSFun_n[2] = rtb_TransferFcn2;
 1602   
 1603     /* MATLAB Function: '<S16>/reference_model' incorporates:
 1604      *  Constant: '<S16>/Constant21'
 1605      *  Constant: '<S16>/Constant22'
 1606      */
 1607     /* MATLAB Function 'Controll system/Control law/Robust MRAC/reference_model': '<S31>:1' */
 1608     /* Initialization */
 1609     /* '<S31>:1:7' if isempty(first) */
 1610     /* System states */
 1611     /* '<S31>:1:17' ypm = zeros(3,1); */
 1612     rtb_ypm[0] = 0.0;
 1613     rtb_ypm[1] = 0.0;
 1614     rtb_ypm[2] = 0.0;
 1615   
 1616     /* '<S31>:1:18' yFm = zeros(3,1); */
 1617     rtb_yFm[0] = 0.0;
 1618     rtb_yFm[1] = 0.0;
 1619     rtb_yFm[2] = 0.0;
 1620   
 1621     /* '<S31>:1:19' xm_dot = zeros(2,3); */
 1622     /* '<S31>:1:20' Fm_dot = zeros(2,3); */
 1623     for (i = 0; i < 6; i++) {
 1624       Controller_B.xm_dot[i] = 0.0;
 1625       Controller_B.Fm_dot[i] = 0.0;
 1626     }
 1627   
 1628     /* Polynomials for control law calculation */
 1629     /* '<S31>:1:23' Z_m = zeros(3,1); */
 1630     rtb_Z_m[0] = 0.0;
 1631     rtb_Z_m[1] = 0.0;
 1632     rtb_Z_m[2] = 0.0;
 1633   
 1634     /* '<S31>:1:24' R_m = zeros(3,3); */
 1635     memset(&rtb_R_m[0], 0, 9U * sizeof(real_T));
 1636   
 1637     /* '<S31>:1:25' k_m_r = zeros(3,1); */
 1638     rtb_k_m_r[0] = 0.0;
 1639     rtb_k_m_r[1] = 0.0;
 1640     rtb_k_m_r[2] = 0.0;
 1641   
 1642     /* Signal indicating force control */
 1643     /* '<S31>:1:28' frc_ctrl = [0 0 0]; */
 1644     rtb_frc_ctrl[0] = 0.0;
 1645     rtb_frc_ctrl[1] = 0.0;
 1646     rtb_frc_ctrl[2] = 0.0;
 1647   
 1648     /* Defining a reference model compensating for environmental dynamics */
 1649     /* '<S31>:1:31' for i = 1:3 */
 1650     for (i = 0; i < 3; i++) {
 1651       /* '<S31>:1:33' xm = y0(:,i); */
 1652       /* '<S31>:1:34' Fm = F0(:,i); */
 1653       /* '<S31>:1:36' if abs(r(i)-eta_p(i)) > e_tol(i) */
 1654       if (fabs(rtb_TmpSignalConversionAtSFun_g[i] -
 1655                rtb_TmpSignalConversionAtSFun_n[i]) >
 1656           Controller_P.Constant22_Value[i]) {
 1657         /* Desired impedance damping factor and reconance frequency */
 1658         /* '<S31>:1:39' zeta = 0.9; */
 1659         rtb_Divide = 0.9;
 1660   
 1661         /* '<S31>:1:40' omega_0 = 1.1; */
 1662         rtb_Sum3_g = 1.1;
 1663   
 1664         /* System matrices of reference model */
 1665         /* '<S31>:1:43' A_m = [-2*zeta*omega_0 1; */
 1666         /* '<S31>:1:44'                             -omega_0^2 0]; */
 1667         /* '<S31>:1:45' B_m = [0 omega_0^2]'; */
 1668         /* '<S31>:1:46' C_m = [1 0]; */
 1669         /* Position control */
 1670         /* '<S31>:1:50' xm_dot(:,i) = A_m*xm + B_m*r_lp(i); */
 1671         Controller_B.xm_dot[i << 1] = (rtb_Integrator2[i << 1] *
 1672           -1.9800000000000002 + rtb_Integrator2[(i << 1) + 1]) + 0.0 *
 1673           rtb_TmpSignalConversionAtSFu_gy[i];
 1674         Controller_B.xm_dot[1 + (i << 1)] = (rtb_Integrator2[(i << 1) + 1] * 0.0 +
 1675           rtb_Integrator2[i << 1] * -1.2100000000000002) + 1.2100000000000002 *
 1676           rtb_TmpSignalConversionAtSFu_gy[i];
 1677   
 1678         /* '<S31>:1:52' ypm(i) = C_m*xm; */
 1679         rtb_ypm[i] = rtb_Integrator2[(i << 1) + 1] * 0.0 + rtb_Integrator2[i << 1];
 1680   
 1681         /* '<S31>:1:54' frc_ctrl(i) = 0; */
 1682         rtb_frc_ctrl[i] = 0.0;
 1683       } else if ((fabs(rtb_TmpSignalConversionAtSFun_d[i]) >
 1684                   Controller_P.Constant21_Value[i << 1]) && (fabs
 1685                   (rtb_TmpSignalConversionAtSFun_g[i] -
 1686                    rtb_TmpSignalConversionAtSFun_n[i]) <
 1687                   Controller_P.Constant22_Value[i])) {
 1688         /* '<S31>:1:57' elseif abs(F_C(i)) > F_tol(1,i) && abs(r(i)-eta_p(i)) < e_tol(i) */
 1689         /* Force control */
 1690         /* Desired impedance damping factor and reconance frequency */
 1691         /* '<S31>:1:62' zeta = 0.9; */
 1692         rtb_Divide = 0.9;
 1693   
 1694         /* '<S31>:1:63' omega_0 = 1.1; */
 1695         rtb_Sum3_g = 1.1;
 1696   
 1697         /* System matrices of reference model */
 1698         /* '<S31>:1:66' A_m = [-2*zeta*omega_0 1; */
 1699         /* '<S31>:1:67'                             -omega_0^2 0]; */
 1700         /* '<S31>:1:68' B_m = [0 omega_0^2]'; */
 1701         /* '<S31>:1:69' C_m = [1 0]; */
 1702         /* '<S31>:1:71' Fm_dot(:,i) = A_m*Fm + B_m*F_C(i); */
 1703         Controller_B.Fm_dot[i << 1] = (rtb_Integrator6[i << 1] *
 1704           -1.9800000000000002 + rtb_Integrator6[(i << 1) + 1]) + 0.0 *
 1705           rtb_TmpSignalConversionAtSFun_d[i];
 1706         Controller_B.Fm_dot[1 + (i << 1)] = (rtb_Integrator6[(i << 1) + 1] * 0.0 +
 1707           rtb_Integrator6[i << 1] * -1.2100000000000002) + 1.2100000000000002 *
 1708           rtb_TmpSignalConversionAtSFun_d[i];
 1709   
 1710         /* '<S31>:1:73' yFm(i) = C_m*Fm; */
 1711         rtb_yFm[i] = rtb_Integrator6[(i << 1) + 1] * 0.0 + rtb_Integrator6[i << 1];
 1712   
 1713         /* '<S31>:1:75' flag(i) = 1; */
 1714         Controller_DWork.flag[i] = 1.0;
 1715   
 1716         /* '<S31>:1:76' frc_ctrl(i) = 1; */
 1717         rtb_frc_ctrl[i] = 1.0;
 1718       } else if ((fabs(rtb_TmpSignalConversionAtSFun_d[i]) <
 1719                   Controller_P.Constant21_Value[(i << 1) + 1]) &&
 1720                  (Controller_DWork.flag[i] == 1.0)) {
 1721         /* '<S31>:1:78' elseif abs(F_C(i)) < F_tol(2,i) && flag(i) == 1 */
 1722         /* Desired impedance damping factor and reconance frequency */
 1723         /* '<S31>:1:81' zeta = 1.1; */
 1724         rtb_Divide = 1.1;
 1725   
 1726         /* '<S31>:1:82' omega_0 = 0.7; */
 1727         rtb_Sum3_g = 0.7;
 1728   
 1729         /* System matrices of reference model */
 1730         /* '<S31>:1:85' A_m = [-2*zeta*omega_0 1; */
 1731         /* '<S31>:1:86'                             -omega_0^2 0]; */
 1732         /* '<S31>:1:87' B_m = [0 omega_0^2]'; */
 1733         /* '<S31>:1:88' C_m = [1 0]; */
 1734         /* Position control */
 1735         /* '<S31>:1:91' xm_dot(:,i) = A_m*xm + B_m*r_lp(i); */
 1736         Controller_B.xm_dot[i << 1] = (rtb_Integrator2[i << 1] * -1.54 +
 1737           rtb_Integrator2[(i << 1) + 1]) + 0.0 * rtb_TmpSignalConversionAtSFu_gy[i];
 1738         Controller_B.xm_dot[1 + (i << 1)] = (rtb_Integrator2[(i << 1) + 1] * 0.0 +
 1739           rtb_Integrator2[i << 1] * -0.48999999999999994) + 0.48999999999999994 *
 1740           rtb_TmpSignalConversionAtSFu_gy[i];
 1741   
 1742         /* '<S31>:1:93' ypm(i) = C_m*xm; */
 1743         rtb_ypm[i] = rtb_Integrator2[(i << 1) + 1] * 0.0 + rtb_Integrator2[i << 1];
 1744   
 1745         /* '<S31>:1:95' flag(i) = 0; */
 1746         Controller_DWork.flag[i] = 0.0;
 1747   
 1748         /* '<S31>:1:96' frc_ctrl(i) = 0; */
 1749         rtb_frc_ctrl[i] = 0.0;
 1750       } else if ((fabs(rtb_TmpSignalConversionAtSFun_d[i]) <
 1751                   Controller_P.Constant21_Value[i << 1]) &&
 1752                  (Controller_DWork.flag[i] == 1.0) && (fabs
 1753                   (rtb_TmpSignalConversionAtSFun_g[i] -
 1754                    rtb_TmpSignalConversionAtSFun_n[i]) <
 1755                   Controller_P.Constant22_Value[i])) {
 1756         /* '<S31>:1:98' elseif abs(F_C(i)) < F_tol(1,i) && flag(i) == 1 && abs(r(i)-eta_p(i)) < e_tol(i) */
 1757         /* Desired impedance damping factor and reconance frequency */
 1758         /* '<S31>:1:101' zeta = 0.9; */
 1759         rtb_Divide = 0.9;
 1760   
 1761         /* '<S31>:1:102' omega_0 = 1.1; */
 1762         rtb_Sum3_g = 1.1;
 1763   
 1764         /* System matrices of reference model */
 1765         /* '<S31>:1:105' A_m = [-2*zeta*omega_0 1; */
 1766         /* '<S31>:1:106'                             -omega_0^2 0]; */
 1767         /* '<S31>:1:107' B_m = [0 omega_0^2]'; */
 1768         /* '<S31>:1:108' C_m = [1 0]; */
 1769         /* Force control */
 1770         /* '<S31>:1:111' Fm_dot(:,i) = A_m*Fm + B_m*F_C(i); */
 1771         Controller_B.Fm_dot[i << 1] = (rtb_Integrator6[i << 1] *
 1772           -1.9800000000000002 + rtb_Integrator6[(i << 1) + 1]) + 0.0 *
 1773           rtb_TmpSignalConversionAtSFun_d[i];
 1774         Controller_B.Fm_dot[1 + (i << 1)] = (rtb_Integrator6[(i << 1) + 1] * 0.0 +
 1775           rtb_Integrator6[i << 1] * -1.2100000000000002) + 1.2100000000000002 *
 1776           rtb_TmpSignalConversionAtSFun_d[i];
 1777   
 1778         /* '<S31>:1:113' yFm(i) = C_m*Fm; */
 1779         rtb_yFm[i] = rtb_Integrator6[(i << 1) + 1] * 0.0 + rtb_Integrator6[i << 1];
 1780   
 1781         /* '<S31>:1:115' frc_ctrl(i) = 1; */
 1782         rtb_frc_ctrl[i] = 1.0;
 1783       } else {
 1784         /* '<S31>:1:117' else */
 1785         /* Desired impedance damping factor and reconance frequency */
 1786         /* '<S31>:1:119' zeta = 0.9; */
 1787         rtb_Divide = 0.9;
 1788   
 1789         /* '<S31>:1:120' omega_0 = 1.1; */
 1790         rtb_Sum3_g = 1.1;
 1791   
 1792         /* System matrices of reference model */
 1793         /* '<S31>:1:123' A_m = [-2*zeta*omega_0 1; */
 1794         /* '<S31>:1:124'                             -omega_0^2 0]; */
 1795         /* '<S31>:1:125' B_m = [0 omega_0^2]'; */
 1796         /* '<S31>:1:126' C_m = [1 0]; */
 1797         /* Position control */
 1798         /* '<S31>:1:130' xm_dot(:,i) = A_m*xm + B_m*r_lp(i); */
 1799         Controller_B.xm_dot[i << 1] = (rtb_Integrator2[i << 1] *
 1800           -1.9800000000000002 + rtb_Integrator2[(i << 1) + 1]) + 0.0 *
 1801           rtb_TmpSignalConversionAtSFu_gy[i];
 1802         Controller_B.xm_dot[1 + (i << 1)] = (rtb_Integrator2[(i << 1) + 1] * 0.0 +
 1803           rtb_Integrator2[i << 1] * -1.2100000000000002) + 1.2100000000000002 *
 1804           rtb_TmpSignalConversionAtSFu_gy[i];
 1805   
 1806         /* '<S31>:1:132' ypm(i) = C_m*xm; */
 1807         rtb_ypm[i] = rtb_Integrator2[(i << 1) + 1] * 0.0 + rtb_Integrator2[i << 1];
 1808   
 1809         /* '<S31>:1:134' frc_ctrl(i) = 0; */
 1810         rtb_frc_ctrl[i] = 0.0;
 1811       }
 1812   
 1813       /* '<S31>:1:138' Z_m(i) = 1; */
 1814       rtb_Z_m[i] = 1.0;
 1815   
 1816       /* '<S31>:1:139' R_m(i,:) = [1 2*zeta*omega_0 omega_0^2]; */
 1817       rtb_R_m[i] = 1.0;
 1818       rtb_R_m[3 + i] = 2.0 * rtb_Divide * rtb_Sum3_g;
 1819       rtb_R_m[6 + i] = rt_powd_snf(rtb_Sum3_g, 2.0);
 1820   
 1821       /* '<S31>:1:140' k_m_r(i) = omega_0^2; */
 1822       rtb_k_m_r[i] = rt_powd_snf(rtb_Sum3_g, 2.0);
 1823     }
 1824   
 1825     for (i = 0; i < 18; i++) {
 1826       /* Integrator: '<S16>/Integrator10' */
 1827       rtb_Integrator10[i] = Controller_X.Integrator10_CSTATE[i];
 1828   
 1829       /* Integrator: '<S16>/Integrator3' */
 1830       rtb_Integrator3[i] = Controller_X.Integrator3_CSTATE[i];
 1831     }
 1832   
 1833     /* MATLAB Function: '<S16>/Calculation of theta_p' incorporates:
 1834      *  Integrator: '<S16>/Integrator10'
 1835      *  Integrator: '<S16>/Integrator3'
 1836      *  SignalConversion: '<S20>/TmpSignal ConversionAt SFunction Inport1'
 1837      *  SignalConversion: '<S20>/TmpSignal ConversionAt SFunction Inport4'
 1838      */
 1839     /* MATLAB Function 'Controll system/Control law/Robust MRAC/Calculation of theta_p': '<S20>:1' */
 1840     /* '<S20>:1:3' kp = zeros(3,3); */
 1841     /* '<S20>:1:4' p21 = zeros(3,3); */
 1842     /* '<S20>:1:5' p22 = zeros(3,3); */
 1843     for (k = 0; k < 9; k++) {
 1844       kp[k] = 0.0;
 1845       p[k] = 0.0;
 1846       p_0[k] = 0.0;
 1847     }
 1848   
 1849     /* '<S20>:1:7' Me_hat = theta_e_hat(:,1:3); */
 1850     /* '<S20>:1:8' De_hat = theta_e_hat(:,4:6); */
 1851     /* '<S20>:1:10' M_hat = theta_ves_hat(1:3,:); */
 1852     /* '<S20>:1:11' D_hat = theta_ves_hat(4:6,:); */
 1853     /* '<S20>:1:13' for i=1:3 */
 1854     /* '<S20>:1:15' if frc_ctrl(i) == 1 */
 1855     if (rtb_frc_ctrl[0] == 1.0) {
 1856       /* '<S20>:1:16' k_value = abs(F_C(i))/(4 - abs(eta_p(2))); */
 1857       rtb_Divide = fabs(rtb_TransferFcn3) / (4.0 - fabs(rtb_Row2));
 1858   
 1859       /* '<S20>:1:18' kp(i,i) = k_value/M_hat(i,i); */
 1860       kp[0] = rtb_Divide / Controller_X.Integrator3_CSTATE[0];
 1861   
 1862       /* '<S20>:1:19' p21(i,i) = D_hat(i,i)/M_hat(i,i); */
 1863       p[0] = Controller_X.Integrator3_CSTATE[3] / Controller_X.Integrator3_CSTATE
 1864         [0];
 1865   
 1866       /* '<S20>:1:20' p22(i,i) = k_value/M_hat(i,i); */
 1867       p_0[0] = rtb_Divide / Controller_X.Integrator3_CSTATE[0];
 1868     } else {
 1869       /* '<S20>:1:21' else */
 1870       /* '<S20>:1:22' kp(i,i) = 1/(M_hat(i,i) + Me_hat(i,i)); */
 1871       kp[0] = 1.0 / (Controller_X.Integrator3_CSTATE[0] +
 1872                      Controller_X.Integrator10_CSTATE[0]);
 1873   
 1874       /* '<S20>:1:23' p21(i,i) = kp(i,i)*(D_hat(i,i) + De_hat(i,i)); */
 1875       p[0] = (Controller_X.Integrator3_CSTATE[3] +
 1876               Controller_X.Integrator10_CSTATE[9]) * kp[0];
 1877     }
 1878   
 1879     /* '<S20>:1:15' if frc_ctrl(i) == 1 */
 1880     if (rtb_frc_ctrl[1] == 1.0) {
 1881       /* '<S20>:1:16' k_value = abs(F_C(i))/(4 - abs(eta_p(2))); */
 1882       rtb_Divide = fabs(rtb_TransferFcn4) / (4.0 - fabs(rtb_Row2));
 1883   
 1884       /* '<S20>:1:18' kp(i,i) = k_value/M_hat(i,i); */
 1885       kp[4] = rtb_Divide / Controller_X.Integrator3_CSTATE[7];
 1886   
 1887       /* '<S20>:1:19' p21(i,i) = D_hat(i,i)/M_hat(i,i); */
 1888       p[4] = Controller_X.Integrator3_CSTATE[10] /
 1889         Controller_X.Integrator3_CSTATE[7];
 1890   
 1891       /* '<S20>:1:20' p22(i,i) = k_value/M_hat(i,i); */
 1892       p_0[4] = rtb_Divide / Controller_X.Integrator3_CSTATE[7];
 1893     } else {
 1894       /* '<S20>:1:21' else */
 1895       /* '<S20>:1:22' kp(i,i) = 1/(M_hat(i,i) + Me_hat(i,i)); */
 1896       kp[4] = 1.0 / (Controller_X.Integrator3_CSTATE[7] +
 1897                      Controller_X.Integrator10_CSTATE[4]);
 1898   
 1899       /* '<S20>:1:23' p21(i,i) = kp(i,i)*(D_hat(i,i) + De_hat(i,i)); */
 1900       p[4] = (Controller_X.Integrator3_CSTATE[10] +
 1901               Controller_X.Integrator10_CSTATE[13]) * kp[4];
 1902     }
 1903   
 1904     /* '<S20>:1:15' if frc_ctrl(i) == 1 */
 1905     if (rtb_frc_ctrl[2] == 1.0) {
 1906       /* '<S20>:1:16' k_value = abs(F_C(i))/(4 - abs(eta_p(2))); */
 1907       rtb_Divide = fabs(rtb_TransferFcn5) / (4.0 - fabs(rtb_Row2));
 1908   
 1909       /* '<S20>:1:18' kp(i,i) = k_value/M_hat(i,i); */
 1910       kp[8] = rtb_Divide / Controller_X.Integrator3_CSTATE[14];
 1911   
 1912       /* '<S20>:1:19' p21(i,i) = D_hat(i,i)/M_hat(i,i); */
 1913       p[8] = Controller_X.Integrator3_CSTATE[17] /
 1914         Controller_X.Integrator3_CSTATE[14];
 1915   
 1916       /* '<S20>:1:20' p22(i,i) = k_value/M_hat(i,i); */
 1917       p_0[8] = rtb_Divide / Controller_X.Integrator3_CSTATE[14];
 1918     } else {
 1919       /* '<S20>:1:21' else */
 1920       /* '<S20>:1:22' kp(i,i) = 1/(M_hat(i,i) + Me_hat(i,i)); */
 1921       kp[8] = 1.0 / (Controller_X.Integrator3_CSTATE[14] +
 1922                      Controller_X.Integrator10_CSTATE[8]);
 1923   
 1924       /* '<S20>:1:23' p21(i,i) = kp(i,i)*(D_hat(i,i) + De_hat(i,i)); */
 1925       p[8] = (Controller_X.Integrator3_CSTATE[17] +
 1926               Controller_X.Integrator10_CSTATE[17]) * kp[8];
 1927     }
 1928   
 1929     /* '<S20>:1:27' mrac_theta_p = [zeros(3,3); kp; p21; p22]; */
 1930     for (k = 0; k < 3; k++) {
 1931       rtb_mrac_theta_p[12 * k] = 0.0;
 1932       rtb_mrac_theta_p[1 + 12 * k] = 0.0;
 1933       rtb_mrac_theta_p[2 + 12 * k] = 0.0;
 1934     }
 1935   
 1936     for (k = 0; k < 3; k++) {
 1937       rtb_mrac_theta_p[3 + 12 * k] = kp[3 * k];
 1938       rtb_mrac_theta_p[4 + 12 * k] = kp[3 * k + 1];
 1939       rtb_mrac_theta_p[5 + 12 * k] = kp[3 * k + 2];
 1940     }
 1941   
 1942     for (k = 0; k < 3; k++) {
 1943       rtb_mrac_theta_p[6 + 12 * k] = p[3 * k];
 1944       rtb_mrac_theta_p[7 + 12 * k] = p[3 * k + 1];
 1945       rtb_mrac_theta_p[8 + 12 * k] = p[3 * k + 2];
 1946     }
 1947   
 1948     for (k = 0; k < 3; k++) {
 1949       rtb_mrac_theta_p[9 + 12 * k] = p_0[3 * k];
 1950       rtb_mrac_theta_p[10 + 12 * k] = p_0[3 * k + 1];
 1951       rtb_mrac_theta_p[11 + 12 * k] = p_0[3 * k + 2];
 1952     }
 1953   
 1954     /* End of MATLAB Function: '<S16>/Calculation of theta_p' */
 1955   
 1956     /* MATLAB Function: '<S16>/Control law parameter calculation' */
 1957     /* MATLAB Function 'Controll system/Control law/Robust MRAC/Control law parameter calculation': '<S21>:1' */
 1958     /* Definition of reference model polynomials. Consider defining these */
 1959     /* elsewhere somehow. */
 1960     /* '<S21>:1:9' if isempty(first) */
 1961     /* Initialization of controller coefficients */
 1962     /* '<S21>:1:24' c_0 = zeros(3,1); */
 1963     rtb_TmpSignalConversionAtSFun_g[0] = 0.0;
 1964     rtb_TmpSignalConversionAtSFun_g[1] = 0.0;
 1965     rtb_TmpSignalConversionAtSFun_g[2] = 0.0;
 1966   
 1967     /* '<S21>:1:25' theta_1 = zeros(3,1); */
 1968     rtb_TmpSignalConversionAtSFu_gy[0] = 0.0;
 1969     rtb_TmpSignalConversionAtSFu_gy[1] = 0.0;
 1970     rtb_TmpSignalConversionAtSFu_gy[2] = 0.0;
 1971   
 1972     /* '<S21>:1:26' theta_2 = zeros(3,1); */
 1973     rtb_TmpSignalConversionAtSFun_d[0] = 0.0;
 1974     rtb_TmpSignalConversionAtSFun_d[1] = 0.0;
 1975     rtb_TmpSignalConversionAtSFun_d[2] = 0.0;
 1976   
 1977     /* '<S21>:1:27' theta_3 = zeros(3,1); */
 1978     rtb_TmpSignalConversionAtSFun_n[0] = 0.0;
 1979     rtb_TmpSignalConversionAtSFun_n[1] = 0.0;
 1980     rtb_TmpSignalConversionAtSFun_n[2] = 0.0;
 1981   
 1982     /* Input from adaptive law */
 1983     /* '<S21>:1:30' k_p_hat = mrac_theta_p(4:6,:); */
 1984     /* '<S21>:1:31' p21 = mrac_theta_p(7:9,:); */
 1985     /* '<S21>:1:32' p22 = mrac_theta_p(10:12,:); */
 1986     /* '<S21>:1:34' for i=1:3 */
 1987     for (i = 0; i < 3; i++) {
 1988       /* '<S21>:1:36' Lambda = conv(Lambda_0,Z_m(i)); */
 1989       rtb_Divide = 0.0;
 1990       for (k = 1; k + 1 < 3; k++) {
 1991         rtb_Divide += Controller_DWork.Lambda_0[k] * rtb_Z_m[i];
 1992       }
 1993   
 1994       rtb_Normalization_idx = rtb_Divide;
 1995   
 1996       /* Define the estimated plant polynomials */
 1997       /* '<S21>:1:39' R_p_hat(i,:) = [1 p21(i,i) p22(i,i)]; */
 1998       Controller_DWork.R_p_hat[i] = 1.0;
 1999       Controller_DWork.R_p_hat[3 + i] = rtb_mrac_theta_p[(12 * i + i) + 6];
 2000       Controller_DWork.R_p_hat[6 + i] = rtb_mrac_theta_p[(12 * i + i) + 9];
 2001   
 2002       /* '<S21>:1:40' Z_p_bar_hat(i) = k_p_hat(i,i); */
 2003       Controller_DWork.Z_p_bar_hat[i] = rtb_mrac_theta_p[(12 * i + i) + 3];
 2004   
 2005       /* '<S21>:1:41' [Q(i,:),R] = deconv(conv(Lambda_0, R_m(i,:)),R_p_hat(i,:)); */
 2006       rtb_VectorConcatenate_p_0[0] = rtb_R_m[i];
 2007       rtb_VectorConcatenate_p_0[1] = rtb_R_m[i + 3];
 2008       rtb_VectorConcatenate_p_0[2] = rtb_R_m[i + 6];
 2009       Controller_conv(Controller_DWork.Lambda_0, rtb_VectorConcatenate_p_0,
 2010                       diophantine_eq);
 2011       omega_idx = Controller_DWork.R_p_hat[i + 3];
 2012       rtb_Normalization_idx_0 = Controller_DWork.R_p_hat[i + 6];
 2013       if (!((!((!rtIsInf(Controller_DWork.R_p_hat[i])) && (!rtIsNaN
 2014               (Controller_DWork.R_p_hat[i])))) || (Controller_DWork.R_p_hat[i] ==
 2015             0.0) || (!(Controller_DWork.R_p_hat[i] != 1.0)))) {
 2016         diophantine_eq[0] /= Controller_DWork.R_p_hat[i];
 2017         diophantine_eq[1] /= Controller_DWork.R_p_hat[i];
 2018         diophantine_eq[2] /= Controller_DWork.R_p_hat[i];
 2019         diophantine_eq[3] /= Controller_DWork.R_p_hat[i];
 2020         omega_idx = Controller_DWork.R_p_hat[i + 3] / Controller_DWork.R_p_hat[i];
 2021         rtb_Normalization_idx_0 = Controller_DWork.R_p_hat[i + 6] /
 2022           Controller_DWork.R_p_hat[i];
 2023       }
 2024   
 2025       rtb_Gain1_l_idx_0 = 0.0;
 2026       rtb_Sum2_i = 0.0;
 2027       rtb_Sum1_pd_idx = 0.0;
 2028       for (k = 0; k < 2; k++) {
 2029         rtb_Gain1_l_idx_1 = (1.0 - (real_T)k) * diophantine_eq[0] +
 2030           rtb_Gain1_l_idx_0;
 2031         rtb_Gain1_l_idx_0 = (1.0 - (real_T)k) * diophantine_eq[1] + rtb_Sum2_i;
 2032         rtb_Sum2_i = (1.0 - (real_T)k) * diophantine_eq[2] + rtb_Sum1_pd_idx;
 2033         rtb_Sum1_pd_idx = (1.0 - (real_T)k) * diophantine_eq[3];
 2034         rtb_Gain1_l_idx_0 -= rtb_Gain1_l_idx_1 * omega_idx;
 2035         rtb_Sum2_i -= rtb_Gain1_l_idx_1 * rtb_Normalization_idx_0;
 2036         q[k] = rtb_Gain1_l_idx_1;
 2037       }
 2038   
 2039       Controller_DWork.Q[i] = q[0];
 2040       Controller_DWork.Q[i + 3] = q[1];
 2041   
 2042       /* Equation for theta_1 */
 2043       /* '<S21>:1:44' theta_1_poly = Lambda - conv(Z_p_bar_hat(i), Q(i,:))/k_p_hat(i,i); */
 2044       rtb_Divide = 0.0;
 2045       idx2 = 1;
 2046       while (idx2 <= 1) {
 2047         rtb_Divide += Controller_DWork.Q[i + 3] * Controller_DWork.Z_p_bar_hat[i];
 2048         idx2 = 2;
 2049       }
 2050   
 2051       /* The Diophantine Equation:  theta_2 + theta_3(s+1) = as + b */
 2052       /* has solution theta_2 = b-a, theta_3 = a */
 2053       /* '<S21>:1:48' diophantine_eq = 1/k_p_hat(i,i)*(conv(Q(i,:),R_p_hat(i,:)) - conv(Lambda_0,R_m(i,:))); */
 2054       rtb_Sum2_i = 1.0 / rtb_mrac_theta_p[(12 * i + i) + 3];
 2055       tmp[0] = Controller_DWork.Q[i];
 2056       tmp[1] = Controller_DWork.Q[i + 3];
 2057       tmp_2[0] = Controller_DWork.R_p_hat[i];
 2058       tmp_2[1] = Controller_DWork.R_p_hat[i + 3];
 2059       tmp_2[2] = Controller_DWork.R_p_hat[i + 6];
 2060       Controller_conv(tmp, tmp_2, tmp_0);
 2061       rtb_VectorConcatenate_p_1[0] = rtb_R_m[i];
 2062       rtb_VectorConcatenate_p_1[1] = rtb_R_m[i + 3];
 2063       rtb_VectorConcatenate_p_1[2] = rtb_R_m[i + 6];
 2064       Controller_conv(Controller_DWork.Lambda_0, rtb_VectorConcatenate_p_1, tmp_1);
 2065       diophantine_eq[2] = (tmp_0[2] - tmp_1[2]) * rtb_Sum2_i;
 2066   
 2067       /* Solve for controller parameters */
 2068       /* '<S21>:1:51' c_0(i) = k_m_r(i)/k_p_hat(i,i); */
 2069       rtb_TmpSignalConversionAtSFun_g[i] = rtb_k_m_r[i] / rtb_mrac_theta_p[(12 * i
 2070         + i) + 3];
 2071   
 2072       /* '<S21>:1:52' theta_1(i) = theta_1_poly(end); */
 2073       rtb_TmpSignalConversionAtSFu_gy[i] = rtb_Normalization_idx - rtb_Divide /
 2074         rtb_mrac_theta_p[(12 * i + i) + 3];
 2075   
 2076       /* '<S21>:1:53' theta_2(i) = diophantine_eq(end)-diophantine_eq(end-1); */
 2077       rtb_TmpSignalConversionAtSFun_d[i] = (tmp_0[3] - tmp_1[3]) * rtb_Sum2_i -
 2078         diophantine_eq[2];
 2079   
 2080       /* '<S21>:1:54' theta_3(i) = diophantine_eq(end-1); */
 2081       rtb_TmpSignalConversionAtSFun_n[i] = diophantine_eq[2];
 2082     }
 2083   
 2084     /* '<S21>:1:58' theta_ctrl_hat = [theta_1 theta_2 theta_3 c_0]; */
 2085     rtb_theta_ctrl_hat[0] = rtb_TmpSignalConversionAtSFu_gy[0];
 2086     rtb_theta_ctrl_hat[1] = rtb_TmpSignalConversionAtSFu_gy[1];
 2087     rtb_theta_ctrl_hat[2] = rtb_TmpSignalConversionAtSFu_gy[2];
 2088     rtb_theta_ctrl_hat[3] = rtb_TmpSignalConversionAtSFun_d[0];
 2089     rtb_theta_ctrl_hat[4] = rtb_TmpSignalConversionAtSFun_d[1];
 2090     rtb_theta_ctrl_hat[5] = rtb_TmpSignalConversionAtSFun_d[2];
 2091     rtb_theta_ctrl_hat[6] = rtb_TmpSignalConversionAtSFun_n[0];
 2092     rtb_theta_ctrl_hat[7] = rtb_TmpSignalConversionAtSFun_n[1];
 2093     rtb_theta_ctrl_hat[8] = rtb_TmpSignalConversionAtSFun_n[2];
 2094     rtb_theta_ctrl_hat[9] = rtb_TmpSignalConversionAtSFun_g[0];
 2095     rtb_theta_ctrl_hat[10] = rtb_TmpSignalConversionAtSFun_g[1];
 2096     rtb_theta_ctrl_hat[11] = rtb_TmpSignalConversionAtSFun_g[2];
 2097   
 2098     /* SignalConversion: '<S28>/TmpSignal ConversionAt SFunction Inport2' incorporates:
 2099      *  Fcn: '<S26>/Row3'
 2100      *  MATLAB Function: '<S16>/control_law'
 2101      */
 2102     rtb_TmpSignalConversionAtSFun_g[0] = rtb_Row1;
 2103     rtb_TmpSignalConversionAtSFun_g[1] = rtb_Row2;
 2104     rtb_TmpSignalConversionAtSFun_g[2] = rtb_TransferFcn2;
 2105   
 2106     /* SignalConversion: '<S28>/TmpSignal ConversionAt SFunction Inport3' incorporates:
 2107      *  MATLAB Function: '<S16>/control_law'
 2108      */
 2109     rtb_TmpSignalConversionAtSFu_gy[0] = rtb_TransferFcn3;
 2110     rtb_TmpSignalConversionAtSFu_gy[1] = rtb_TransferFcn4;
 2111     rtb_TmpSignalConversionAtSFu_gy[2] = rtb_TransferFcn5;
 2112   
 2113     /* SignalConversion: '<S28>/TmpSignal ConversionAt SFunction Inport4' incorporates:
 2114      *  MATLAB Function: '<S16>/control_law'
 2115      */
 2116     rtb_TmpSignalConversionAtSFun_d[0] = rtb_TransferFcn_j;
 2117     rtb_TmpSignalConversionAtSFun_d[1] = rtb_TransferFcn1_n;
 2118     rtb_TmpSignalConversionAtSFun_d[2] = rtb_TransferFcn2_b;
 2119   
 2120     /* MATLAB Function: '<S16>/control_law' incorporates:
 2121      *  Constant: '<S16>/Constant16'
 2122      *  Constant: '<S16>/Constant18'
 2123      *  Fcn: '<S26>/Row3'
 2124      *  Integrator: '<S16>/Integrator1'
 2125      *  Integrator: '<S16>/Integrator10'
 2126      *  SignalConversion: '<S28>/TmpSignal ConversionAt SFunction Inport2'
 2127      *  SignalConversion: '<S28>/TmpSignal ConversionAt SFunction Inport3'
 2128      */
 2129     /* MATLAB Function 'Controll system/Control law/Robust MRAC/control_law': '<S28>:1' */
 2130     /* '<S28>:1:4' theta_1 = theta_ctrl_r_hat(:,1); */
 2131     /* '<S28>:1:5' theta_2 = theta_ctrl_r_hat(:,2); */
 2132     /* '<S28>:1:6' theta_3 = theta_ctrl_r_hat(:,3); */
 2133     /* '<S28>:1:7' c_0 = theta_ctrl_r_hat(:,4); */
 2134     /* '<S28>:1:9' F_T = zeros(3,1); */
 2135     rtb_Z_m[0] = 0.0;
 2136     rtb_Z_m[1] = 0.0;
 2137     rtb_Z_m[2] = 0.0;
 2138   
 2139     /* '<S28>:1:10' omega1_dot = zeros(1,3); */
 2140     /* '<S28>:1:11' omega2_dot = zeros(1,3); */
 2141     /* '<S28>:1:12' omega3_dot = zeros(1,3); */
 2142     /* Constant ocean current and environmental damping */
 2143     /* '<S28>:1:15' nu_c = -0.1; */
 2144     /* '<S28>:1:16' D_e_hat = theta_e_hat(:,4:6); */
 2145     /* Control laws */
 2146     /* '<S28>:1:19' for i=1:3 */
 2147     for (i = 0; i < 3; i++) {
 2148       /* '<S28>:1:21' if frc_ctrl(i) == 1 */
 2149       if (rtb_frc_ctrl[i] == 1.0) {
 2150         /* Force control. Divide theta by c_0 as we are dealing with force reference */
 2151         /* '<S28>:1:24' theta = [theta_1(i) theta_2(i) theta_3(i) c_0(i)]'./c_0(i); */
 2152         rtb_Gain1_l_idx_1 = rtb_theta_ctrl_hat[i] / rtb_theta_ctrl_hat[9 + i];
 2153         rtb_Gain1_l_idx_0 = rtb_theta_ctrl_hat[3 + i] / rtb_theta_ctrl_hat[9 + i];
 2154         rtb_Sum2_i = rtb_theta_ctrl_hat[6 + i] / rtb_theta_ctrl_hat[9 + i];
 2155         rtb_Sum1_pd_idx = rtb_theta_ctrl_hat[9 + i] / rtb_theta_ctrl_hat[9 + i];
 2156   
 2157         /* '<S28>:1:25' omega = [omega0(1,i) omega0(3,i) F_C(i) F_C(i)]'; */
 2158         rtb_Normalization_idx_0 = Controller_X.Integrator1_CSTATE_m[3 * i];
 2159         rtb_Normalization_idx = Controller_X.Integrator1_CSTATE_m[3 * i + 2];
 2160         omega_idx = rtb_TmpSignalConversionAtSFu_gy[i];
 2161         rtb_Sum3_g = rtb_TmpSignalConversionAtSFu_gy[i];
 2162       } else {
 2163         /* '<S28>:1:27' else */
 2164         /* Position control. */
 2165         /* '<S28>:1:30' theta = [theta_1(i) theta_2(i) theta_3(i) c_0(i)]'; */
 2166         rtb_Gain1_l_idx_1 = rtb_theta_ctrl_hat[i];
 2167         rtb_Gain1_l_idx_0 = rtb_theta_ctrl_hat[3 + i];
 2168         rtb_Sum2_i = rtb_theta_ctrl_hat[6 + i];
 2169         rtb_Sum1_pd_idx = rtb_theta_ctrl_hat[9 + i];
 2170   
 2171         /* '<S28>:1:31' omega = [omega0(1,i) omega0(2,i) eta_p(i) r(i)]'; */
 2172         rtb_Normalization_idx_0 = Controller_X.Integrator1_CSTATE_m[3 * i];
 2173         rtb_Normalization_idx = Controller_X.Integrator1_CSTATE_m[3 * i + 1];
 2174         omega_idx = rtb_TmpSignalConversionAtSFun_g[i];
 2175         rtb_Sum3_g = rtb_TmpSignalConversionAtSFun_d[i];
 2176       }
 2177   
 2178       /* '<S28>:1:35' F_T(i) = theta'*omega; */
 2179       rtb_Z_m[i] = ((rtb_Gain1_l_idx_1 * rtb_Normalization_idx_0 +
 2180                      rtb_Gain1_l_idx_0 * rtb_Normalization_idx) + rtb_Sum2_i *
 2181                     omega_idx) + rtb_Sum1_pd_idx * rtb_Sum3_g;
 2182     }
 2183   
 2184     /* '<S28>:1:40' for i = 1:3 */
 2185     /* Filtered thruster force for position/force control */
 2186     /* '<S28>:1:43' if i == 1 */
 2187     /* Subtract damping in surge to counteract constant ocean current */
 2188     /* '<S28>:1:45' omega1_dot(i) = F*omega0(1,i)+g*(F_T(i) + nu_c*D_e_hat(i,i)); */
 2189     /* Filtered position for position control */
 2190     /* '<S28>:1:51' omega2_dot(i) = F*omega0(2,i)+g*eta_p(i); */
 2191     /* Filtered environmental force for force control */
 2192     /* '<S28>:1:54' omega3_dot(i) = F*omega0(3,i)+g*F_C(i); */
 2193     /* Filtered thruster force for position/force control */
 2194     /* '<S28>:1:43' if i == 1 */
 2195     /* '<S28>:1:46' else */
 2196     /* '<S28>:1:47' omega1_dot(i) = F*omega0(1,i)+g*F_T(i); */
 2197     /* Filtered position for position control */
 2198     /* '<S28>:1:51' omega2_dot(i) = F*omega0(2,i)+g*eta_p(i); */
 2199     /* Filtered environmental force for force control */
 2200     /* '<S28>:1:54' omega3_dot(i) = F*omega0(3,i)+g*F_C(i); */
 2201     /* Filtered thruster force for position/force control */
 2202     /* '<S28>:1:43' if i == 1 */
 2203     /* '<S28>:1:46' else */
 2204     /* '<S28>:1:47' omega1_dot(i) = F*omega0(1,i)+g*F_T(i); */
 2205     /* Filtered position for position control */
 2206     /* '<S28>:1:51' omega2_dot(i) = F*omega0(2,i)+g*eta_p(i); */
 2207     /* Filtered environmental force for force control */
 2208     /* '<S28>:1:54' omega3_dot(i) = F*omega0(3,i)+g*F_C(i); */
 2209     /* '<S28>:1:58' omega0_dot = [omega1_dot; omega2_dot; omega3_dot]; */
 2210     Controller_B.omega0_dot[0] = (-0.1 * Controller_X.Integrator10_CSTATE[9] +
 2211       rtb_Z_m[0]) * Controller_P.Constant18_Value + Controller_P.Constant16_Value *
 2212       Controller_X.Integrator1_CSTATE_m[0];
 2213     Controller_B.omega0_dot[3] = Controller_P.Constant16_Value *
 2214       Controller_X.Integrator1_CSTATE_m[3] + Controller_P.Constant18_Value *
 2215       rtb_Z_m[1];
 2216     Controller_B.omega0_dot[6] = Controller_P.Constant16_Value *
 2217       Controller_X.Integrator1_CSTATE_m[6] + Controller_P.Constant18_Value *
 2218       rtb_Z_m[2];
 2219     Controller_B.omega0_dot[1] = Controller_P.Constant16_Value *
 2220       Controller_X.Integrator1_CSTATE_m[1] + Controller_P.Constant18_Value *
 2221       rtb_Row1;
 2222     Controller_B.omega0_dot[4] = Controller_P.Constant16_Value *
 2223       Controller_X.Integrator1_CSTATE_m[4] + Controller_P.Constant18_Value *
 2224       rtb_Row2;
 2225     Controller_B.omega0_dot[7] = Controller_P.Constant16_Value *
 2226       Controller_X.Integrator1_CSTATE_m[7] + Controller_P.Constant18_Value *
 2227       rtb_TransferFcn2;
 2228     Controller_B.omega0_dot[2] = Controller_P.Constant16_Value *
 2229       Controller_X.Integrator1_CSTATE_m[2] + Controller_P.Constant18_Value *
 2230       rtb_TransferFcn3;
 2231     Controller_B.omega0_dot[5] = Controller_P.Constant16_Value *
 2232       Controller_X.Integrator1_CSTATE_m[5] + Controller_P.Constant18_Value *
 2233       rtb_TransferFcn4;
 2234     Controller_B.omega0_dot[8] = Controller_P.Constant16_Value *
 2235       Controller_X.Integrator1_CSTATE_m[8] + Controller_P.Constant18_Value *
 2236       rtb_TransferFcn5;
 2237   
 2238     /* SignalConversion: '<S133>/ConcatBufferAtVector ConcatenateIn1' incorporates:
 2239      *  Inport: '<Root>/Ki_x'
 2240      */
 2241     rtb_VectorConcatenate_p[0] = Controller_U.Ki_x;
 2242   
 2243     /* SignalConversion: '<S133>/ConcatBufferAtVector ConcatenateIn2' incorporates:
 2244      *  Constant: '<S6>/Constant2'
 2245      */
 2246     rtb_VectorConcatenate_p[1] = Controller_P.Constant2_Value;
 2247   
 2248     /* SignalConversion: '<S133>/ConcatBufferAtVector ConcatenateIn3' incorporates:
 2249      *  Constant: '<S6>/Constant1'
 2250      */
 2251     rtb_VectorConcatenate_p[2] = Controller_P.Constant1_Value;
 2252   
 2253     /* SignalConversion: '<S133>/ConcatBufferAtVector ConcatenateIn4' incorporates:
 2254      *  Constant: '<S6>/Constant2'
 2255      */
 2256     rtb_VectorConcatenate_p[3] = Controller_P.Constant2_Value;
 2257   
 2258     /* SignalConversion: '<S133>/ConcatBufferAtVector ConcatenateIn5' incorporates:
 2259      *  Inport: '<Root>/Ki_y'
 2260      */
 2261     rtb_VectorConcatenate_p[4] = Controller_U.Ki_y;
 2262   
 2263     /* SignalConversion: '<S133>/ConcatBufferAtVector ConcatenateIn6' incorporates:
 2264      *  Constant: '<S6>/Constant1'
 2265      */
 2266     rtb_VectorConcatenate_p[5] = Controller_P.Constant1_Value;
 2267   
 2268     /* SignalConversion: '<S133>/ConcatBufferAtVector ConcatenateIn7' incorporates:
 2269      *  Constant: '<S6>/Constant2'
 2270      */
 2271     rtb_VectorConcatenate_p[6] = Controller_P.Constant2_Value;
 2272   
 2273     /* SignalConversion: '<S133>/ConcatBufferAtVector ConcatenateIn8' incorporates:
 2274      *  Constant: '<S6>/Constant1'
 2275      */
 2276     rtb_VectorConcatenate_p[7] = Controller_P.Constant1_Value;
 2277   
 2278     /* SignalConversion: '<S133>/ConcatBufferAtVector ConcatenateIn9' incorporates:
 2279      *  Inport: '<Root>/Ki_psi'
 2280      */
 2281     rtb_VectorConcatenate_p[8] = Controller_U.Ki_psi;
 2282   
 2283     /* SignalConversion: '<S132>/ConcatBufferAtVector ConcatenateIn1' incorporates:
 2284      *  Inport: '<Root>/Kp_x'
 2285      */
 2286     rtb_VectorConcatenate_c[0] = Controller_U.Kp_x;
 2287   
 2288     /* SignalConversion: '<S132>/ConcatBufferAtVector ConcatenateIn2' incorporates:
 2289      *  Constant: '<S5>/Constant2'
 2290      */
 2291     rtb_VectorConcatenate_c[1] = Controller_P.Constant2_Value_h;
 2292   
 2293     /* SignalConversion: '<S132>/ConcatBufferAtVector ConcatenateIn3' incorporates:
 2294      *  Constant: '<S5>/Constant1'
 2295      */
 2296     rtb_VectorConcatenate_c[2] = Controller_P.Constant1_Value_c;
 2297   
 2298     /* SignalConversion: '<S132>/ConcatBufferAtVector ConcatenateIn4' incorporates:
 2299      *  Constant: '<S5>/Constant2'
 2300      */
 2301     rtb_VectorConcatenate_c[3] = Controller_P.Constant2_Value_h;
 2302   
 2303     /* SignalConversion: '<S132>/ConcatBufferAtVector ConcatenateIn5' incorporates:
 2304      *  Inport: '<Root>/Kp_y'
 2305      */
 2306     rtb_VectorConcatenate_c[4] = Controller_U.Kp_y;
 2307   
 2308     /* SignalConversion: '<S132>/ConcatBufferAtVector ConcatenateIn6' incorporates:
 2309      *  Constant: '<S5>/Constant1'
 2310      */
 2311     rtb_VectorConcatenate_c[5] = Controller_P.Constant1_Value_c;
 2312   
 2313     /* SignalConversion: '<S132>/ConcatBufferAtVector ConcatenateIn7' incorporates:
 2314      *  Constant: '<S5>/Constant2'
 2315      */
 2316     rtb_VectorConcatenate_c[6] = Controller_P.Constant2_Value_h;
 2317   
 2318     /* SignalConversion: '<S132>/ConcatBufferAtVector ConcatenateIn8' incorporates:
 2319      *  Constant: '<S5>/Constant1'
 2320      */
 2321     rtb_VectorConcatenate_c[7] = Controller_P.Constant1_Value_c;
 2322   
 2323     /* SignalConversion: '<S132>/ConcatBufferAtVector ConcatenateIn9' incorporates:
 2324      *  Inport: '<Root>/Kp_psi'
 2325      */
 2326     rtb_VectorConcatenate_c[8] = Controller_U.Kp_psi;
 2327   
 2328     /* SignalConversion: '<S15>/TmpSignal ConversionAtProduct1Inport2' incorporates:
 2329      *  Fcn: '<S19>/Row1'
 2330      *  Fcn: '<S19>/Row2'
 2331      *  Fcn: '<S19>/Row3'
 2332      */
 2333     Controller_B.TmpSignalConversionAtProduct1In[0] = cos(rtb_TransferFcn2) *
 2334       rtb_D[0] + sin(rtb_TransferFcn2) * rtb_D[1];
 2335     Controller_B.TmpSignalConversionAtProduct1In[1] = -sin(rtb_TransferFcn2) *
 2336       rtb_D[0] + cos(rtb_TransferFcn2) * rtb_D[1];
 2337     Controller_B.TmpSignalConversionAtProduct1In[2] = rtb_D[2];
 2338   
 2339     /* SignalConversion: '<S134>/ConcatBufferAtVector ConcatenateIn1' incorporates:
 2340      *  Inport: '<Root>/Kd_u'
 2341      */
 2342     rtb_VectorConcatenate_h[0] = Controller_U.Kd_u;
 2343   
 2344     /* SignalConversion: '<S134>/ConcatBufferAtVector ConcatenateIn2' incorporates:
 2345      *  Constant: '<S7>/Constant2'
 2346      */
 2347     rtb_VectorConcatenate_h[1] = Controller_P.Constant2_Value_c;
 2348   
 2349     /* SignalConversion: '<S134>/ConcatBufferAtVector ConcatenateIn3' incorporates:
 2350      *  Constant: '<S7>/Constant1'
 2351      */
 2352     rtb_VectorConcatenate_h[2] = Controller_P.Constant1_Value_g;
 2353   
 2354     /* SignalConversion: '<S134>/ConcatBufferAtVector ConcatenateIn4' incorporates:
 2355      *  Constant: '<S7>/Constant2'
 2356      */
 2357     rtb_VectorConcatenate_h[3] = Controller_P.Constant2_Value_c;
 2358   
 2359     /* SignalConversion: '<S134>/ConcatBufferAtVector ConcatenateIn5' incorporates:
 2360      *  Inport: '<Root>/Kd_v'
 2361      */
 2362     rtb_VectorConcatenate_h[4] = Controller_U.Kd_v;
 2363   
 2364     /* SignalConversion: '<S134>/ConcatBufferAtVector ConcatenateIn6' incorporates:
 2365      *  Constant: '<S7>/Constant1'
 2366      */
 2367     rtb_VectorConcatenate_h[5] = Controller_P.Constant1_Value_g;
 2368   
 2369     /* SignalConversion: '<S134>/ConcatBufferAtVector ConcatenateIn7' incorporates:
 2370      *  Constant: '<S7>/Constant2'
 2371      */
 2372     rtb_VectorConcatenate_h[6] = Controller_P.Constant2_Value_c;
 2373   
 2374     /* SignalConversion: '<S134>/ConcatBufferAtVector ConcatenateIn8' incorporates:
 2375      *  Constant: '<S7>/Constant1'
 2376      */
 2377     rtb_VectorConcatenate_h[7] = Controller_P.Constant1_Value_g;
 2378   
 2379     /* SignalConversion: '<S134>/ConcatBufferAtVector ConcatenateIn9' incorporates:
 2380      *  Inport: '<Root>/Kd_r'
 2381      */
 2382     rtb_VectorConcatenate_h[8] = Controller_U.Kd_r;
 2383   
 2384     /* Sum: '<S15>/Sum4' incorporates:
 2385      *  Constant: '<S15>/Constant4'
 2386      *  Product: '<S15>/Product2'
 2387      */
 2388     rtb_Normalization_idx_0 = Controller_P.Constant4_Value[0] - rtb_Sum_be[0];
 2389     rtb_Normalization_idx = Controller_P.Constant4_Value[1] - rtb_Sum_be[1];
 2390     omega_idx = Controller_P.Constant4_Value[2] - Controller_B.Integrator[2];
 2391   
 2392     /* Product: '<S15>/Product2' */
 2393     for (k = 0; k < 3; k++) {
 2394       rtb_D[k] = rtb_VectorConcatenate_h[k + 6] * omega_idx +
 2395         (rtb_VectorConcatenate_h[k + 3] * rtb_Normalization_idx +
 2396          rtb_VectorConcatenate_h[k] * rtb_Normalization_idx_0);
 2397     }
 2398   
 2399     /* Integrator: '<S33>/Integrator1' */
 2400     rtb_Integrator1_i = Controller_X.Integrator1_CSTATE_f;
 2401   
 2402     /* Sum: '<S38>/Sum2' incorporates:
 2403      *  Constant: '<S38>/rho'
 2404      *  Integrator: '<S33>/Integrator1'
 2405      *  Product: '<S38>/Product'
 2406      *  Trigonometry: '<S38>/Trigonometric Function'
 2407      */
 2408     rtb_Sum2_i = cos(Controller_X.Integrator1_CSTATE_f) * Controller_P.rho_Value +
 2409       rtb_Sum_be[0];
 2410   
 2411     /* Sum: '<S36>/Sum' */
 2412     rtb_Sum_k = rtb_Sum2_a_idx_1 - rtb_Sum2_i;
 2413   
 2414     /* Sum: '<S38>/Sum3' incorporates:
 2415      *  Constant: '<S38>/rho'
 2416      *  Integrator: '<S33>/Integrator1'
 2417      *  Product: '<S38>/Product1'
 2418      *  Trigonometry: '<S38>/Trigonometric Function1'
 2419      */
 2420     rtb_Sum3_g = Controller_P.rho_Value * sin(Controller_X.Integrator1_CSTATE_f) +
 2421       rtb_Sum_be[1];
 2422   
 2423     /* Sum: '<S36>/Sum2' */
 2424     rtb_TransferFcn2_h = rtb_Sum2_a_idx_0 - rtb_Sum3_g;
 2425   
 2426     /* Sum: '<S36>/Sum3' incorporates:
 2427      *  Math: '<S36>/Math Function1'
 2428      *  Math: '<S36>/Math Function2'
 2429      */
 2430     rtb_TransferFcn1_o = rtb_Sum_k * rtb_Sum_k + rtb_TransferFcn2_h *
 2431       rtb_TransferFcn2_h;
 2432   
 2433     /* Sqrt: '<S36>/Sqrt' */
 2434     rtb_TransferFcn1_o = sqrt(rtb_TransferFcn1_o);
 2435   
 2436     /* Product: '<S41>/Product' incorporates:
 2437      *  Constant: '<S41>/rho'
 2438      *  Trigonometry: '<S41>/Trigonometric Function'
 2439      */
 2440     rtb_Gain5_g = cos(rtb_TransferFcn2) * Controller_P.rho_Value_o;
 2441   
 2442     /* Sum: '<S35>/Sum4' */
 2443     rtb_Normalization_idx_0 = rtb_Sum2_i - rtb_Sum2_a_idx_1;
 2444     rtb_Normalization_idx = rtb_Sum3_g - rtb_Sum2_a_idx_0;
 2445   
 2446     /* S-Function (sdsp2norm2): '<S35>/Normalization' */
 2447     rtb_Divide = rtb_Normalization_idx_0 * rtb_Normalization_idx_0 +
 2448       rtb_Normalization_idx * rtb_Normalization_idx;
 2449     rtb_Divide = 1.0 / (sqrt(rtb_Divide) + Controller_P.Normalization_BIAS);
 2450     rtb_Normalization_idx_0 *= rtb_Divide;
 2451     rtb_Normalization_idx *= rtb_Divide;
 2452   
 2453     /* DotProduct: '<S35>/Dot Product' incorporates:
 2454      *  Constant: '<S41>/rho'
 2455      *  Product: '<S41>/Product1'
 2456      *  SignalConversion: '<S35>/TmpSignal ConversionAtDot ProductInport1'
 2457      *  Trigonometry: '<S41>/Trigonometric Function1'
 2458      */
 2459     omega_idx = Controller_P.rho_Value_o * sin(rtb_TransferFcn2) *
 2460       rtb_Normalization_idx + rtb_Gain5_g * rtb_Normalization_idx_0;
 2461   
 2462     /* Integrator: '<S33>/Integrator' */
 2463     Controller_B.Integrator_n = Controller_X.Integrator_CSTATE_c;
 2464   
 2465     /* Product: '<S40>/Divide' incorporates:
 2466      *  Constant: '<S37>/rho1'
 2467      *  Gain: '<S37>/Gain1'
 2468      *  Inport: '<Root>/eta_d_x'
 2469      *  Integrator: '<S33>/Integrator1'
 2470      *  Product: '<S37>/Product2'
 2471      *  Product: '<S37>/Product3'
 2472      *  Product: '<S40>/Product1'
 2473      *  Product: '<S40>/Product2'
 2474      *  Sum: '<S40>/Sum'
 2475      *  Sum: '<S40>/Sum1'
 2476      *  Sum: '<S40>/Sum2'
 2477      *  Sum: '<S40>/Sum3'
 2478      *  Sum: '<S40>/Sum4'
 2479      *  Trigonometry: '<S37>/Trigonometric Function2'
 2480      *  Trigonometry: '<S37>/Trigonometric Function3'
 2481      */
 2482     rtb_Divide = ((Controller_U.eta_d_x - Controller_P.Gain1_Gain_jx *
 2483                    Controller_B.Integrator_n * sin
 2484                    (Controller_X.Integrator1_CSTATE_f) * Controller_P.rho1_Value) *
 2485                   (rtb_Sum2_a_idx_1 - rtb_Sum2_i) + (rtb_Gain_ia -
 2486       Controller_P.rho1_Value * cos(Controller_X.Integrator1_CSTATE_f) *
 2487       Controller_B.Integrator_n) * (rtb_Sum2_a_idx_0 - rtb_Sum3_g)) /
 2488       rtb_TransferFcn1_o;
 2489   
 2490     /* Gain: '<S32>/Gain' */
 2491     rtb_Gain_ia *= Controller_P.Gain_Gain_k;
 2492   
 2493     /* Sum: '<S34>/Sum3' incorporates:
 2494      *  Constant: '<S34>/Constant'
 2495      *  Gain: '<S34>/Gain'
 2496      *  Gain: '<S34>/Gain1'
 2497      *  Gain: '<S34>/Gain2'
 2498      *  Gain: '<S34>/Gain3'
 2499      *  Sum: '<S34>/Sum'
 2500      *  Sum: '<S34>/Sum1'
 2501      *  Sum: '<S34>/Sum2'
 2502      *  Sum: '<S34>/Sum4'
 2503      *  Trigonometry: '<S34>/Trigonometric Function'
 2504      */
 2505     rtb_Gain5_g = (rt_atan2d_snf((rtb_Sum2_a_idx_0 - rtb_Sum3_g) *
 2506       Controller_P.Gain3_Gain, (rtb_Sum2_a_idx_1 - rtb_Sum2_i) *
 2507       Controller_P.Gain2_Gain_l) - rtb_TransferFcn2) * Controller_P.Gain_Gain_i +
 2508       (Controller_P.Constant_Value - rtb_Gain1_f) * Controller_P.Gain1_Gain_p;
 2509   
 2510     /* Gain: '<S17>/Gain2' incorporates:
 2511      *  Inport: '<Root>/AFF_on'
 2512      *  Product: '<S17>/Product3'
 2513      *  Sum: '<S17>/Sum2'
 2514      */
 2515     for (k = 0; k < 3; k++) {
 2516       tmp_2[k] = Controller_P.Gain2_Gain_d[k + 6] * (Controller_B.Gain[2] *
 2517         Controller_U.AFF_on) + (Controller_P.Gain2_Gain_d[k + 3] *
 2518         (Controller_B.Gain[1] * Controller_U.AFF_on) + Controller_B.Gain[0] *
 2519         Controller_U.AFF_on * Controller_P.Gain2_Gain_d[k]);
 2520     }
 2521   
 2522     /* End of Gain: '<S17>/Gain2' */
 2523   
 2524     /* Step: '<S11>/Step' */
 2525     if (Controller_M->Timing.t[0] < Controller_P.Step_Time_l) {
 2526       rtb_Step = Controller_P.Step_Y0_k;
 2527     } else {
 2528       rtb_Step = Controller_P.Step_YFinal_j;
 2529     }
 2530   
 2531     /* End of Step: '<S11>/Step' */
 2532   
 2533     /* Product: '<S15>/Product' incorporates:
 2534      *  Integrator: '<S15>/Integrator'
 2535      *  Product: '<S18>/Product4'
 2536      */
 2537     for (k = 0; k < 3; k++) {
 2538       rtb_VectorConcatenate_p_0[k] = rtb_VectorConcatenate_p[k + 6] *
 2539         Controller_X.Integrator_CSTATE_p[2] + (rtb_VectorConcatenate_p[k + 3] *
 2540         Controller_X.Integrator_CSTATE_p[1] + rtb_VectorConcatenate_p[k] *
 2541         Controller_X.Integrator_CSTATE_p[0]);
 2542     }
 2543   
 2544     /* End of Product: '<S15>/Product' */
 2545   
 2546     /* Sum: '<S15>/Sum1' incorporates:
 2547      *  Constant: '<S18>/Constant1'
 2548      *  Gain: '<S11>/Gain'
 2549      *  Gain: '<S18>/Gain'
 2550      *  Inport: '<Root>/AFF_on'
 2551      *  Inport: '<Root>/Ki_on'
 2552      *  Product: '<S15>/Product1'
 2553      *  Product: '<S18>/Product3'
 2554      *  Product: '<S18>/Product4'
 2555      *  Sum: '<S18>/Sum2'
 2556      */
 2557     for (k = 0; k < 3; k++) {
 2558       rtb_VectorConcatenate_p_1[k] = (((rtb_VectorConcatenate_c[k + 3] *
 2559         Controller_B.TmpSignalConversionAtProduct1In[1] +
 2560         rtb_VectorConcatenate_c[k] * Controller_B.TmpSignalConversionAtProduct1In
 2561         [0]) + rtb_VectorConcatenate_c[k + 6] *
 2562         Controller_B.TmpSignalConversionAtProduct1In[2]) +
 2563         ((rtb_VectorConcatenate_p_0[k] * Controller_U.Ki_on +
 2564           Controller_P.Constant1_Value_e[k]) + Controller_P.Gain_Gain_b *
 2565          Controller_B.Gain[k] * Controller_U.AFF_on)) + rtb_D[k];
 2566     }
 2567   
 2568     /* End of Sum: '<S15>/Sum1' */
 2569   
 2570     /* Product: '<S11>/Product3' incorporates:
 2571      *  Constant: '<S35>/rho'
 2572      *  Constant: '<S35>/rho1'
 2573      *  DotProduct: '<S35>/Dot Product'
 2574      *  Gain: '<S11>/Gain'
 2575      *  Gain: '<S11>/Gain1'
 2576      *  Gain: '<S11>/Gain2'
 2577      *  Gain: '<S35>/Kd'
 2578      *  Gain: '<S35>/Kp'
 2579      *  Product: '<S35>/Product'
 2580      *  Sum: '<S11>/Sum'
 2581      *  Sum: '<S17>/Sum2'
 2582      *  Sum: '<S35>/Sum1'
 2583      *  Sum: '<S35>/Sum2'
 2584      *  Sum: '<S35>/Sum3'
 2585      */
 2586     Controller_B.Product3[0] = ((((Controller_P.rho_Value_c - rtb_TransferFcn1_o) *
 2587       omega_idx * Controller_P.Kp_Gain + (Controller_P.rho1_Value_e - rtb_Divide) *
 2588       Controller_P.Kd_Gain) + tmp_2[0]) * Controller_P.Gain1_Gain_k +
 2589       (Controller_P.Gain2_Gain * rtb_Z_m[0] + Controller_P.Gain_Gain_c *
 2590        rtb_VectorConcatenate_p_1[0])) * rtb_Step;
 2591     Controller_B.Product3[1] = ((Controller_P.Gain2_Gain * rtb_Z_m[1] +
 2592       Controller_P.Gain_Gain_c * rtb_VectorConcatenate_p_1[1]) + (rtb_Gain_ia +
 2593       tmp_2[1]) * Controller_P.Gain1_Gain_k) * rtb_Step;
 2594     Controller_B.Product3[2] = ((Controller_P.Gain2_Gain * rtb_Z_m[2] +
 2595       Controller_P.Gain_Gain_c * rtb_VectorConcatenate_p_1[2]) + (rtb_Gain5_g +
 2596       tmp_2[2]) * Controller_P.Gain1_Gain_k) * rtb_Step;
 2597   
 2598     /* MATLAB Function: '<S14>/MATLAB Function' */
 2599     Controller_MATLABFunction();
 2600   
 2601     /* Gain: '<S14>/Gain' */
 2602     for (k = 0; k < 3; k++) {
 2603       rtb_Gain_pk[k] = 0.0;
 2604       for (i = 0; i < 12; i++) {
 2605         rtb_Gain_pk[k] += Controller_P.Gain_Gain_l[3 * i + k] * Controller_B.u[i];
 2606       }
 2607     }
 2608   
 2609     /* End of Gain: '<S14>/Gain' */
 2610   
 2611     /* Outport: '<Root>/X_com' */
 2612     Controller_Y.X_com = rtb_Gain_pk[0];
 2613   
 2614     /* Outport: '<Root>/Y_com' */
 2615     Controller_Y.Y_com = rtb_Gain_pk[1];
 2616   
 2617     /* Outport: '<Root>/N_com' */
 2618     Controller_Y.N_com = rtb_Gain_pk[2];
 2619   
 2620     /* SignalConversion: '<S47>/TmpSignal ConversionAtsincosInport1' incorporates:
 2621      *  Inport: '<Root>/pitch'
 2622      *  Inport: '<Root>/roll'
 2623      *  Inport: '<Root>/yaw'
 2624      */
 2625     rtb_k_m_r[0] = Controller_U.yaw;
 2626     rtb_k_m_r[1] = Controller_U.pitch;
 2627     rtb_k_m_r[2] = Controller_U.roll;
 2628   
 2629     /* Trigonometry: '<S136>/sincos' incorporates:
 2630      *  Inport: '<Root>/pitch'
 2631      *  Inport: '<Root>/roll'
 2632      *  Inport: '<Root>/yaw'
 2633      *  SignalConversion: '<S47>/TmpSignal ConversionAtsincosInport1'
 2634      */
 2635     rtb_D[0] = sin(Controller_U.yaw);
 2636     rtb_Gain1_l_idx_1 = cos(Controller_U.yaw);
 2637     rtb_D[1] = sin(Controller_U.pitch);
 2638     rtb_Gain1_l_idx_0 = cos(Controller_U.pitch);
 2639     rtb_D[2] = sin(Controller_U.roll);
 2640     rtb_Gain1_l_idx = cos(Controller_U.roll);
 2641   
 2642     /* Fcn: '<S136>/Fcn11' */
 2643     rtb_VectorConcatenate_n[0] = rtb_Gain1_l_idx_0 * rtb_Gain1_l_idx_1;
 2644   
 2645     /* Fcn: '<S136>/Fcn21' */
 2646     rtb_VectorConcatenate_n[1] = rtb_D[2] * rtb_D[1] * rtb_Gain1_l_idx_1 -
 2647       rtb_Gain1_l_idx * rtb_D[0];
 2648   
 2649     /* Fcn: '<S136>/Fcn31' */
 2650     rtb_VectorConcatenate_n[2] = rtb_Gain1_l_idx * rtb_D[1] * rtb_Gain1_l_idx_1 +
 2651       rtb_D[2] * rtb_D[0];
 2652   
 2653     /* Fcn: '<S136>/Fcn12' */
 2654     rtb_VectorConcatenate_n[3] = rtb_Gain1_l_idx_0 * rtb_D[0];
 2655   
 2656     /* Fcn: '<S136>/Fcn22' */
 2657     rtb_VectorConcatenate_n[4] = rtb_D[2] * rtb_D[1] * rtb_D[0] + rtb_Gain1_l_idx *
 2658       rtb_Gain1_l_idx_1;
 2659   
 2660     /* Fcn: '<S136>/Fcn32' */
 2661     rtb_VectorConcatenate_n[5] = rtb_Gain1_l_idx * rtb_D[1] * rtb_D[0] - rtb_D[2] *
 2662       rtb_Gain1_l_idx_1;
 2663   
 2664     /* Fcn: '<S136>/Fcn13' */
 2665     rtb_VectorConcatenate_n[6] = -rtb_D[1];
 2666   
 2667     /* Fcn: '<S136>/Fcn23' */
 2668     rtb_VectorConcatenate_n[7] = rtb_D[2] * rtb_Gain1_l_idx_0;
 2669   
 2670     /* Fcn: '<S136>/Fcn33' */
 2671     rtb_VectorConcatenate_n[8] = rtb_Gain1_l_idx * rtb_Gain1_l_idx_0;
 2672   
 2673     /* Step: '<S4>/Step' */
 2674     if (Controller_M->Timing.t[0] < Controller_P.Step_Time_j) {
 2675       rtb_Normalization_idx = Controller_P.Step_Y0_g;
 2676     } else {
 2677       rtb_Normalization_idx = Controller_P.Step_YFinal_g;
 2678     }
 2679   
 2680     /* End of Step: '<S4>/Step' */
 2681   
 2682     /* SignalConversion: '<S10>/TmpSignal ConversionAtProductInport2' incorporates:
 2683      *  Inport: '<Root>/u'
 2684      *  Inport: '<Root>/v_r'
 2685      *  Product: '<S4>/Product'
 2686      *  Sum: '<S4>/Sum'
 2687      */
 2688     rtb_Gain1_l_idx_1 = Controller_U.v_r * rtb_Normalization_idx + Controller_U.u;
 2689   
 2690     /* Product: '<S10>/Product' incorporates:
 2691      *  Inport: '<Root>/v'
 2692      *  Inport: '<Root>/w'
 2693      *  SignalConversion: '<S10>/TmpSignal ConversionAtProductInport2'
 2694      */
 2695     for (k = 0; k < 3; k++) {
 2696       rtb_D[k] = rtb_VectorConcatenate_n[k + 6] * Controller_U.w +
 2697         (rtb_VectorConcatenate_n[k + 3] * Controller_U.v +
 2698          rtb_VectorConcatenate_n[k] * rtb_Gain1_l_idx_1);
 2699     }
 2700   
 2701     /* End of Product: '<S10>/Product' */
 2702   
 2703     /* Outport: '<Root>/nu_sim_u' */
 2704     Controller_Y.nu_sim_u = rtb_D[0];
 2705   
 2706     /* Outport: '<Root>/nu_sim_v' */
 2707     Controller_Y.nu_sim_v = rtb_D[1];
 2708   
 2709     /* Trigonometry: '<S137>/sincos' incorporates:
 2710      *  Inport: '<Root>/pitch'
 2711      *  Inport: '<Root>/roll'
 2712      *  Inport: '<Root>/yaw'
 2713      *  SignalConversion: '<S47>/TmpSignal ConversionAtsincosInport1'
 2714      */
 2715     rtb_Gain1_l_idx_1 = sin(Controller_U.yaw);
 2716     rtb_D[0] = cos(Controller_U.yaw);
 2717     rtb_Gain1_l_idx_0 = sin(Controller_U.pitch);
 2718     rtb_D[1] = cos(Controller_U.pitch);
 2719     rtb_Gain1_l_idx = sin(Controller_U.roll);
 2720     rtb_D[2] = cos(Controller_U.roll);
 2721   
 2722     /* Fcn: '<S137>/Fcn11' */
 2723     rtb_VectorConcatenate_ck[0] = rtb_D[1] * rtb_D[0];
 2724   
 2725     /* Fcn: '<S137>/Fcn21' */
 2726     rtb_VectorConcatenate_ck[1] = rtb_Gain1_l_idx * rtb_Gain1_l_idx_0 * rtb_D[0] -
 2727       rtb_D[2] * rtb_Gain1_l_idx_1;
 2728   
 2729     /* Fcn: '<S137>/Fcn31' */
 2730     rtb_VectorConcatenate_ck[2] = rtb_D[2] * rtb_Gain1_l_idx_0 * rtb_D[0] +
 2731       rtb_Gain1_l_idx * rtb_Gain1_l_idx_1;
 2732   
 2733     /* Fcn: '<S137>/Fcn12' */
 2734     rtb_VectorConcatenate_ck[3] = rtb_D[1] * rtb_Gain1_l_idx_1;
 2735   
 2736     /* Fcn: '<S137>/Fcn22' */
 2737     rtb_VectorConcatenate_ck[4] = rtb_Gain1_l_idx * rtb_Gain1_l_idx_0 *
 2738       rtb_Gain1_l_idx_1 + rtb_D[2] * rtb_D[0];
 2739   
 2740     /* Fcn: '<S137>/Fcn32' */
 2741     rtb_VectorConcatenate_ck[5] = rtb_D[2] * rtb_Gain1_l_idx_0 * rtb_Gain1_l_idx_1
 2742       - rtb_Gain1_l_idx * rtb_D[0];
 2743   
 2744     /* Fcn: '<S137>/Fcn13' */
 2745     rtb_VectorConcatenate_ck[6] = -rtb_Gain1_l_idx_0;
 2746   
 2747     /* Fcn: '<S137>/Fcn23' */
 2748     rtb_VectorConcatenate_ck[7] = rtb_Gain1_l_idx * rtb_D[1];
 2749   
 2750     /* Fcn: '<S137>/Fcn33' */
 2751     rtb_VectorConcatenate_ck[8] = rtb_D[2] * rtb_D[1];
 2752   
 2753     /* SignalConversion: '<S46>/TmpSignal ConversionAtProduct2Inport2' incorporates:
 2754      *  Inport: '<Root>/p'
 2755      *  Inport: '<Root>/q'
 2756      *  Inport: '<Root>/r'
 2757      */
 2758     rtb_Gain1_l_idx_1 = Controller_U.p;
 2759     rtb_Gain1_l_idx_0 = Controller_U.q;
 2760     rtb_Gain1_l_idx = Controller_U.r;
 2761   
 2762     /* Product: '<S10>/Product1' incorporates:
 2763      *  Inport: '<Root>/p'
 2764      *  Inport: '<Root>/q'
 2765      *  Inport: '<Root>/r'
 2766      *  SignalConversion: '<S46>/TmpSignal ConversionAtProduct2Inport2'
 2767      */
 2768     for (k = 0; k < 3; k++) {
 2769       rtb_D[k] = rtb_VectorConcatenate_ck[k + 6] * Controller_U.r +
 2770         (rtb_VectorConcatenate_ck[k + 3] * Controller_U.q +
 2771          rtb_VectorConcatenate_ck[k] * Controller_U.p);
 2772     }
 2773   
 2774     /* End of Product: '<S10>/Product1' */
 2775   
 2776     /* Outport: '<Root>/nu_sim_r' */
 2777     Controller_Y.nu_sim_r = rtb_D[2];
 2778   
 2779     /* Sum: '<S3>/Sum' incorporates:
 2780      *  Constant: '<S3>/Constant'
 2781      *  Inport: '<Root>/N'
 2782      *  Integrator: '<S3>/Integrator'
 2783      *  Sum: '<S3>/Sum1'
 2784      */
 2785     rtb_Step = (Controller_X.Integrator_CSTATE_j - Controller_P.Constant_Value_g)
 2786       + Controller_U.N;
 2787   
 2788     /* Outport: '<Root>/eta_sim_N' */
 2789     Controller_Y.eta_sim_N = rtb_Step;
 2790   
 2791     /* Outport: '<Root>/eta_sim_E' incorporates:
 2792      *  Inport: '<Root>/E'
 2793      */
 2794     Controller_Y.eta_sim_E = Controller_U.E;
 2795   
 2796     /* Outport: '<Root>/eta_sim_psi' incorporates:
 2797      *  Inport: '<Root>/yaw'
 2798      */
 2799     Controller_Y.eta_sim_psi = Controller_U.yaw;
 2800   
 2801     /* Trigonometry: '<S128>/sincos' incorporates:
 2802      *  Inport: '<Root>/pitch'
 2803      *  Inport: '<Root>/roll'
 2804      *  Inport: '<Root>/yaw'
 2805      *  SignalConversion: '<S47>/TmpSignal ConversionAtsincosInport1'
 2806      */
 2807     rtb_D[0] = sin(Controller_U.yaw);
 2808     rtb_Sum1_pd_idx = cos(Controller_U.yaw);
 2809     rtb_D[1] = sin(Controller_U.pitch);
 2810     rtb_Gain1_f = cos(Controller_U.pitch);
 2811     rtb_D[2] = sin(Controller_U.roll);
 2812     rtb_Gain_ia = cos(Controller_U.roll);
 2813   
 2814     /* Fcn: '<S128>/Fcn11' */
 2815     rtb_VectorConcatenate_k[0] = rtb_Gain1_f * rtb_Sum1_pd_idx;
 2816   
 2817     /* Fcn: '<S128>/Fcn21' */
 2818     rtb_VectorConcatenate_k[1] = rtb_D[2] * rtb_D[1] * rtb_Sum1_pd_idx -
 2819       rtb_Gain_ia * rtb_D[0];
 2820   
 2821     /* Fcn: '<S128>/Fcn31' */
 2822     rtb_VectorConcatenate_k[2] = rtb_Gain_ia * rtb_D[1] * rtb_Sum1_pd_idx + rtb_D
 2823       [2] * rtb_D[0];
 2824   
 2825     /* Fcn: '<S128>/Fcn12' */
 2826     rtb_VectorConcatenate_k[3] = rtb_Gain1_f * rtb_D[0];
 2827   
 2828     /* Fcn: '<S128>/Fcn22' */
 2829     rtb_VectorConcatenate_k[4] = rtb_D[2] * rtb_D[1] * rtb_D[0] + rtb_Gain_ia *
 2830       rtb_Sum1_pd_idx;
 2831   
 2832     /* Fcn: '<S128>/Fcn32' */
 2833     rtb_VectorConcatenate_k[5] = rtb_Gain_ia * rtb_D[1] * rtb_D[0] - rtb_D[2] *
 2834       rtb_Sum1_pd_idx;
 2835   
 2836     /* Fcn: '<S128>/Fcn13' */
 2837     rtb_VectorConcatenate_k[6] = -rtb_D[1];
 2838   
 2839     /* Fcn: '<S128>/Fcn23' */
 2840     rtb_VectorConcatenate_k[7] = rtb_D[2] * rtb_Gain1_f;
 2841   
 2842     /* Fcn: '<S128>/Fcn33' */
 2843     rtb_VectorConcatenate_k[8] = rtb_Gain_ia * rtb_Gain1_f;
 2844   
 2845     /* Sum: '<S2>/Sum1' incorporates:
 2846      *  Constant: '<S2>/Gravity'
 2847      *  Inport: '<Root>/w_d'
 2848      */
 2849     rtb_Gain5_g = Controller_U.w_d + Controller_P.Gravity_Value;
 2850   
 2851     /* Product: '<S2>/Product' incorporates:
 2852      *  Inport: '<Root>/u_d'
 2853      *  Inport: '<Root>/v_d'
 2854      *  SignalConversion: '<S2>/TmpSignal ConversionAtProductInport2'
 2855      */
 2856     for (k = 0; k < 3; k++) {
 2857       rtb_D[k] = rtb_VectorConcatenate_k[k + 6] * rtb_Gain5_g +
 2858         (rtb_VectorConcatenate_k[k + 3] * Controller_U.v_d +
 2859          rtb_VectorConcatenate_k[k] * Controller_U.u_d);
 2860     }
 2861   
 2862     /* End of Product: '<S2>/Product' */
 2863   
 2864     /* Outport: '<Root>/nu_d_sim_u' */
 2865     Controller_Y.nu_d_sim_u = rtb_D[0];
 2866   
 2867     /* Outport: '<Root>/nu_d_sim_v' */
 2868     Controller_Y.nu_d_sim_v = rtb_D[1];
 2869   
 2870     /* Trigonometry: '<S129>/sincos' incorporates:
 2871      *  Inport: '<Root>/pitch'
 2872      *  Inport: '<Root>/roll'
 2873      *  Inport: '<Root>/yaw'
 2874      *  SignalConversion: '<S47>/TmpSignal ConversionAtsincosInport1'
 2875      */
 2876     rtb_Sum1_pd_idx = sin(Controller_U.yaw);
 2877     rtb_D[0] = cos(Controller_U.yaw);
 2878     rtb_Gain1_f = sin(Controller_U.pitch);
 2879     rtb_D[1] = cos(Controller_U.pitch);
 2880     rtb_Gain_ia = sin(Controller_U.roll);
 2881     rtb_D[2] = cos(Controller_U.roll);
 2882   
 2883     /* Fcn: '<S129>/Fcn11' */
 2884     rtb_VectorConcatenate_nu[0] = rtb_D[1] * rtb_D[0];
 2885   
 2886     /* Fcn: '<S129>/Fcn21' */
 2887     rtb_VectorConcatenate_nu[1] = rtb_Gain_ia * rtb_Gain1_f * rtb_D[0] - rtb_D[2] *
 2888       rtb_Sum1_pd_idx;
 2889   
 2890     /* Fcn: '<S129>/Fcn31' */
 2891     rtb_VectorConcatenate_nu[2] = rtb_D[2] * rtb_Gain1_f * rtb_D[0] + rtb_Gain_ia *
 2892       rtb_Sum1_pd_idx;
 2893   
 2894     /* Fcn: '<S129>/Fcn12' */
 2895     rtb_VectorConcatenate_nu[3] = rtb_D[1] * rtb_Sum1_pd_idx;
 2896   
 2897     /* Fcn: '<S129>/Fcn22' */
 2898     rtb_VectorConcatenate_nu[4] = rtb_Gain_ia * rtb_Gain1_f * rtb_Sum1_pd_idx +
 2899       rtb_D[2] * rtb_D[0];
 2900   
 2901     /* Fcn: '<S129>/Fcn32' */
 2902     rtb_VectorConcatenate_nu[5] = rtb_D[2] * rtb_Gain1_f * rtb_Sum1_pd_idx -
 2903       rtb_Gain_ia * rtb_D[0];
 2904   
 2905     /* Fcn: '<S129>/Fcn13' */
 2906     rtb_VectorConcatenate_nu[6] = -rtb_Gain1_f;
 2907   
 2908     /* Fcn: '<S129>/Fcn23' */
 2909     rtb_VectorConcatenate_nu[7] = rtb_Gain_ia * rtb_D[1];
 2910   
 2911     /* Fcn: '<S129>/Fcn33' */
 2912     rtb_VectorConcatenate_nu[8] = rtb_D[2] * rtb_D[1];
 2913   
 2914     /* SignalConversion: '<S43>/TmpSignal ConversionAtProduct1Inport2' incorporates:
 2915      *  Inport: '<Root>/p_d'
 2916      *  Inport: '<Root>/q_d'
 2917      *  Inport: '<Root>/r_d'
 2918      */
 2919     rtb_Sum1_pd_idx = Controller_U.p_d;
 2920     rtb_Gain1_f = Controller_U.q_d;
 2921     rtb_Gain_ia = Controller_U.r_d;
 2922   
 2923     /* Product: '<S2>/Product1' incorporates:
 2924      *  Inport: '<Root>/p_d'
 2925      *  Inport: '<Root>/q_d'
 2926      *  Inport: '<Root>/r_d'
 2927      *  Outport: '<Root>/nu_d_sim_r'
 2928      *  SignalConversion: '<S43>/TmpSignal ConversionAtProduct1Inport2'
 2929      */
 2930     for (k = 0; k < 3; k++) {
 2931       rtb_VectorConcatenate_p_0[k] = rtb_VectorConcatenate_nu[k + 6] *
 2932         Controller_U.r_d + (rtb_VectorConcatenate_nu[k + 3] * Controller_U.q_d +
 2933                             rtb_VectorConcatenate_nu[k] * Controller_U.p_d);
 2934     }
 2935   
 2936     /* End of Product: '<S2>/Product1' */
 2937   
 2938     /* Outport: '<Root>/nu_d_sim_r' */
 2939     Controller_Y.nu_d_sim_r = rtb_VectorConcatenate_p_0[2];
 2940   
 2941     /* Integrator: '<S84>/Integrator1' */
 2942     Controller_B.Integrator1[0] = Controller_X.Integrator1_CSTATE_k[0];
 2943     Controller_B.Integrator1[1] = Controller_X.Integrator1_CSTATE_k[1];
 2944     Controller_B.Integrator1[2] = Controller_X.Integrator1_CSTATE_k[2];
 2945   
 2946     /* Integrator: '<S84>/Integrator' */
 2947     Controller_B.Integrator_o[0] = Controller_X.Integrator_CSTATE_px[0];
 2948     Controller_B.Integrator_o[1] = Controller_X.Integrator_CSTATE_px[1];
 2949     Controller_B.Integrator_o[2] = Controller_X.Integrator_CSTATE_px[2];
 2950   
 2951     /* Gain: '<S84>/D' */
 2952     for (k = 0; k < 3; k++) {
 2953       rtb_D[k] = Controller_P.D_Gain[k + 6] * Controller_B.Integrator_o[2] +
 2954         (Controller_P.D_Gain[k + 3] * Controller_B.Integrator_o[1] +
 2955          Controller_P.D_Gain[k] * Controller_B.Integrator_o[0]);
 2956     }
 2957   
 2958     /* End of Gain: '<S84>/D' */
 2959   
 2960     /* Gain: '<S13>/Gain' incorporates:
 2961      *  Inport: '<Root>/E'
 2962      */
 2963     rtb_Integrator6[0] = Controller_P.Gain_Gain_f * rtb_Step;
 2964     rtb_Integrator6[1] = Controller_P.Gain_Gain_f * Controller_U.E;
 2965     rtb_Integrator6[3] = Controller_P.Gain_Gain_f * rtb_TransferFcn;
 2966     rtb_Integrator6[4] = Controller_P.Gain_Gain_f * rtb_TransferFcn1;
 2967     rtb_Integrator6[5] = Controller_P.Gain_Gain_f * rtb_TransferFcn2;
 2968   
 2969     /* SignalConversion: '<S120>/TmpSignal ConversionAtsincosInport1' incorporates:
 2970      *  Gain: '<S13>/Gain'
 2971      */
 2972     rtb_sincos_o1_e_idx = Controller_P.Gain_Gain_f * rtb_TransferFcn1;
 2973   
 2974     /* Trigonometry: '<S123>/sincos' incorporates:
 2975      *  SignalConversion: '<S120>/TmpSignal ConversionAtsincosInport1'
 2976      */
 2977     rtb_sincos_o2[0] = sin(rtb_Integrator6[5]);
 2978     rtb_Integrator2_n_idx_1 = cos(rtb_Integrator6[5]);
 2979     rtb_sincos_o2[1] = sin(rtb_Integrator6[4]);
 2980     rtb_Integrator2_n_idx_0 = cos(rtb_Integrator6[4]);
 2981     rtb_sincos_o2[2] = sin(rtb_Integrator6[3]);
 2982     rtb_Integrator2_n_idx = cos(rtb_Integrator6[3]);
 2983   
 2984     /* Fcn: '<S123>/Fcn11' */
 2985     rtb_VectorConcatenate_kf[0] = rtb_Integrator2_n_idx_0 *
 2986       rtb_Integrator2_n_idx_1;
 2987   
 2988     /* Fcn: '<S123>/Fcn21' */
 2989     rtb_VectorConcatenate_kf[1] = rtb_sincos_o2[2] * rtb_sincos_o2[1] *
 2990       rtb_Integrator2_n_idx_1 - rtb_Integrator2_n_idx * rtb_sincos_o2[0];
 2991   
 2992     /* Fcn: '<S123>/Fcn31' */
 2993     rtb_VectorConcatenate_kf[2] = rtb_Integrator2_n_idx * rtb_sincos_o2[1] *
 2994       rtb_Integrator2_n_idx_1 + rtb_sincos_o2[2] * rtb_sincos_o2[0];
 2995   
 2996     /* Fcn: '<S123>/Fcn12' */
 2997     rtb_VectorConcatenate_kf[3] = rtb_Integrator2_n_idx_0 * rtb_sincos_o2[0];
 2998   
 2999     /* Fcn: '<S123>/Fcn22' */
 3000     rtb_VectorConcatenate_kf[4] = rtb_sincos_o2[2] * rtb_sincos_o2[1] *
 3001       rtb_sincos_o2[0] + rtb_Integrator2_n_idx * rtb_Integrator2_n_idx_1;
 3002   
 3003     /* Fcn: '<S123>/Fcn32' */
 3004     rtb_VectorConcatenate_kf[5] = rtb_Integrator2_n_idx * rtb_sincos_o2[1] *
 3005       rtb_sincos_o2[0] - rtb_sincos_o2[2] * rtb_Integrator2_n_idx_1;
 3006   
 3007     /* Fcn: '<S123>/Fcn13' */
 3008     rtb_VectorConcatenate_kf[6] = -rtb_sincos_o2[1];
 3009   
 3010     /* Fcn: '<S123>/Fcn23' */
 3011     rtb_VectorConcatenate_kf[7] = rtb_sincos_o2[2] * rtb_Integrator2_n_idx_0;
 3012   
 3013     /* Fcn: '<S123>/Fcn33' */
 3014     rtb_VectorConcatenate_kf[8] = rtb_Integrator2_n_idx * rtb_Integrator2_n_idx_0;
 3015   
 3016     /* Integrator: '<S119>/Integrator2' */
 3017     rtb_Integrator2_n_idx_1 = Controller_X.Integrator2_CSTATE_i[0];
 3018     rtb_Integrator2_n_idx_0 = Controller_X.Integrator2_CSTATE_i[1];
 3019     rtb_Integrator2_n_idx = Controller_X.Integrator2_CSTATE_i[2];
 3020   
 3021     /* Product: '<S117>/Product' incorporates:
 3022      *  Gain: '<S84>/Gain4'
 3023      *  Integrator: '<S119>/Integrator2'
 3024      */
 3025     for (k = 0; k < 3; k++) {
 3026       rtb_VectorConcatenate_p_0[k] = rtb_VectorConcatenate_kf[k + 6] *
 3027         Controller_X.Integrator2_CSTATE_i[2] + (rtb_VectorConcatenate_kf[k + 3] *
 3028         Controller_X.Integrator2_CSTATE_i[1] + rtb_VectorConcatenate_kf[k] *
 3029         Controller_X.Integrator2_CSTATE_i[0]);
 3030     }
 3031   
 3032     /* End of Product: '<S117>/Product' */
 3033   
 3034     /* Gain: '<S84>/Gain4' */
 3035     for (k = 0; k < 3; k++) {
 3036       Controller_B.Gain4[k] = 0.0;
 3037       Controller_B.Gain4[k] += Controller_P.Gain4_Gain[k] *
 3038         rtb_VectorConcatenate_p_0[0];
 3039       Controller_B.Gain4[k] += Controller_P.Gain4_Gain[k + 3] *
 3040         rtb_VectorConcatenate_p_0[1];
 3041       Controller_B.Gain4[k] += Controller_P.Gain4_Gain[k + 6] *
 3042         rtb_VectorConcatenate_p_0[2];
 3043     }
 3044   
 3045     /* Gain: '<S13>/Gain7' incorporates:
 3046      *  Constant: '<S85>/Constant'
 3047      *  Sum: '<S84>/Sum4'
 3048      */
 3049     Controller_B.Gain7[0] = Controller_P.Gain7_Gain * Controller_B.Integrator1[0];
 3050     Controller_B.Gain7[1] = Controller_P.Gain7_Gain * Controller_B.Integrator1[1];
 3051     Controller_B.Gain7[2] = Controller_P.Gain7_Gain *
 3052       Controller_P.Constant_Value_m;
 3053     Controller_B.Gain7[3] = Controller_P.Gain7_Gain *
 3054       Controller_P.Constant_Value_m;
 3055     Controller_B.Gain7[4] = Controller_P.Gain7_Gain *
 3056       Controller_P.Constant_Value_m;
 3057     Controller_B.Gain7[5] = Controller_P.Gain7_Gain * Controller_B.Integrator1[2];
 3058     Controller_B.Gain7[6] = Controller_P.Gain7_Gain * Controller_B.Integrator_o[0];
 3059     Controller_B.Gain7[7] = Controller_P.Gain7_Gain * Controller_B.Integrator_o[1];
 3060     Controller_B.Gain7[8] = Controller_P.Gain7_Gain *
 3061       Controller_P.Constant_Value_m;
 3062     Controller_B.Gain7[9] = Controller_P.Gain7_Gain *
 3063       Controller_P.Constant_Value_m;
 3064     Controller_B.Gain7[10] = Controller_P.Gain7_Gain *
 3065       Controller_P.Constant_Value_m;
 3066     Controller_B.Gain7[11] = Controller_P.Gain7_Gain * Controller_B.Integrator_o[2];
 3067     Controller_B.Gain7[12] = (Controller_B.Gain4[0] - rtb_D[0]) *
 3068       Controller_P.Gain7_Gain;
 3069     Controller_B.Gain7[13] = (Controller_B.Gain4[1] - rtb_D[1]) *
 3070       Controller_P.Gain7_Gain;
 3071     Controller_B.Gain7[14] = (Controller_B.Gain4[2] - rtb_D[2]) *
 3072       Controller_P.Gain7_Gain;
 3073   
 3074     /* Outport: '<Root>/eta_e_N_NL' */
 3075     Controller_Y.eta_e_N_NL = Controller_B.Gain7[0];
 3076   
 3077     /* Outport: '<Root>/eta_e_E_NL' */
 3078     Controller_Y.eta_e_E_NL = Controller_B.Gain7[1];
 3079   
 3080     /* Outport: '<Root>/eta_e_psi_NL' */
 3081     Controller_Y.eta_e_psi_NL = Controller_B.Gain7[5];
 3082   
 3083     /* Outport: '<Root>/nu_e_u_NL' */
 3084     Controller_Y.nu_e_u_NL = Controller_B.Gain7[6];
 3085   
 3086     /* Outport: '<Root>/nu_e_v_NL' */
 3087     Controller_Y.nu_e_v_NL = Controller_B.Gain7[7];
 3088   
 3089     /* Outport: '<Root>/nu_e_r_NL' */
 3090     Controller_Y.nu_e_r_NL = Controller_B.Gain7[11];
 3091   
 3092     /* Outport: '<Root>/d_e_x_NL' */
 3093     Controller_Y.d_e_x_NL = Controller_B.Gain7[12];
 3094   
 3095     /* Outport: '<Root>/d_e_y_NL' */
 3096     Controller_Y.d_e_y_NL = Controller_B.Gain7[13];
 3097   
 3098     /* Outport: '<Root>/d_e_psi_NL' */
 3099     Controller_Y.d_e_psi_NL = Controller_B.Gain7[14];
 3100   
 3101     /* TransportDelay: '<S93>/Transport Delay' */
 3102     {
 3103       real_T **uBuffer = (real_T**)
 3104         &Controller_DWork.TransportDelay_PWORK.TUbufferPtrs[0];
 3105       real_T **tBuffer = (real_T**)
 3106         &Controller_DWork.TransportDelay_PWORK.TUbufferPtrs[3];
 3107       real_T simTime = Controller_M->Timing.t[0];
 3108       real_T tMinusDelay ;
 3109   
 3110       {
 3111         int_T i1;
 3112         real_T *y0 = Controller_B.TransportDelay;
 3113         int_T *iw_Tail = &Controller_DWork.TransportDelay_IWORK.Tail[0];
 3114         int_T *iw_Head = &Controller_DWork.TransportDelay_IWORK.Head[0];
 3115         int_T *iw_Last = &Controller_DWork.TransportDelay_IWORK.Last[0];
 3116         int_T *iw_CircularBufSize =
 3117           &Controller_DWork.TransportDelay_IWORK.CircularBufSize[0];
 3118         for (i1=0; i1 < 3; i1++) {
 3119           tMinusDelay = ((Controller_P.TransportDelay_Delay > 0.0) ?
 3120                          Controller_P.TransportDelay_Delay : 0.0);
 3121           tMinusDelay = simTime - tMinusDelay;
 3122           y0[i1] = rt_TDelayInterpolate(
 3123             tMinusDelay,
 3124             0.0,
 3125             *tBuffer,
 3126             *uBuffer,
 3127             iw_CircularBufSize[i1],
 3128             &iw_Last[i1],
 3129             iw_Tail[i1],
 3130             iw_Head[i1],
 3131             Controller_P.TransportDelay_InitOutput,
 3132             0,
 3133             0);
 3134           tBuffer++;
 3135           uBuffer++;
 3136         }
 3137       }
 3138     }
 3139   
 3140     /* Gain: '<S13>/Gain3' incorporates:
 3141      *  Inport: '<Root>/D'
 3142      *  Inport: '<Root>/E'
 3143      */
 3144     Controller_B.Gain3[0] = Controller_P.Gain3_Gain_d * rtb_Step;
 3145     Controller_B.Gain3[1] = Controller_P.Gain3_Gain_d * Controller_U.E;
 3146     Controller_B.Gain3[2] = Controller_P.Gain3_Gain_d * Controller_U.D;
 3147     Controller_B.Gain3[3] = Controller_P.Gain3_Gain_d * rtb_TransferFcn;
 3148     Controller_B.Gain3[4] = Controller_P.Gain3_Gain_d * rtb_TransferFcn1;
 3149     Controller_B.Gain3[5] = Controller_P.Gain3_Gain_d * rtb_TransferFcn2;
 3150   
 3151     /* Integrator: '<S93>/Integrator1' */
 3152     if (rtmIsMajorTimeStep(Controller_M)) {
 3153       ZCEventType zcEvent;
 3154       zcEvent = rt_ZCFcn(RISING_ZERO_CROSSING,
 3155                          &Controller_PrevZCSigState.Integrator1_Reset_ZCE,
 3156                          Controller_B.TransportDelay[0]);
 3157   
 3158       /* evaluate zero-crossings */
 3159       if (zcEvent || Controller_DWork.Integrator1_IWORK.IcNeedsLoading) {
 3160         Controller_X.Integrator1_CSTATE_g = Controller_B.Gain3[0];
 3161       }
 3162     }
 3163   
 3164     Controller_B.Integrator1_h = Controller_X.Integrator1_CSTATE_g;
 3165   
 3166     /* Integrator: '<S93>/Integrator2' */
 3167     if (rtmIsMajorTimeStep(Controller_M)) {
 3168       ZCEventType zcEvent;
 3169       zcEvent = rt_ZCFcn(RISING_ZERO_CROSSING,
 3170                          &Controller_PrevZCSigState.Integrator2_Reset_ZCE,
 3171                          Controller_B.TransportDelay[1]);
 3172   
 3173       /* evaluate zero-crossings */
 3174       if (zcEvent || Controller_DWork.Integrator2_IWORK.IcNeedsLoading) {
 3175         Controller_X.Integrator2_CSTATE_n = Controller_B.Gain3[1];
 3176       }
 3177     }
 3178   
 3179     Controller_B.Integrator2 = Controller_X.Integrator2_CSTATE_n;
 3180   
 3181     /* Integrator: '<S93>/Integrator3' */
 3182     if (rtmIsMajorTimeStep(Controller_M)) {
 3183       ZCEventType zcEvent;
 3184       zcEvent = rt_ZCFcn(RISING_ZERO_CROSSING,
 3185                          &Controller_PrevZCSigState.Integrator3_Reset_ZCE,
 3186                          Controller_B.TransportDelay[2]);
 3187   
 3188       /* evaluate zero-crossings */
 3189       if (zcEvent || Controller_DWork.Integrator3_IWORK.IcNeedsLoading) {
 3190         Controller_X.Integrator3_CSTATE_m = Controller_B.Gain3[5];
 3191       }
 3192     }
 3193   
 3194     Controller_B.Integrator3 = Controller_X.Integrator3_CSTATE_m;
 3195   
 3196     /* Sum: '<S82>/Sum3' */
 3197     rtb_Sum3_c_idx_1 = Controller_B.Gain3[0] - Controller_B.Integrator1_h;
 3198     rtb_Sum3_c_idx_0 = Controller_B.Gain3[1] - Controller_B.Integrator2;
 3199     rtb_Sum3_c_idx = Controller_B.Gain3[5] - Controller_B.Integrator3;
 3200   
 3201     /* Saturate: '<S92>/Saturation' */
 3202     if (rtb_Sum3_c_idx_1 >= Controller_P.Saturation_UpperSat) {
 3203       rtb_Normalization_idx_0 = Controller_P.Saturation_UpperSat;
 3204     } else if (rtb_Sum3_c_idx_1 <= Controller_P.Saturation_LowerSat) {
 3205       rtb_Normalization_idx_0 = Controller_P.Saturation_LowerSat;
 3206     } else {
 3207       rtb_Normalization_idx_0 = rtb_Sum3_c_idx_1;
 3208     }
 3209   
 3210     if (rtb_Sum3_c_idx_0 >= Controller_P.Saturation_UpperSat) {
 3211       rtb_Normalization_idx = Controller_P.Saturation_UpperSat;
 3212     } else if (rtb_Sum3_c_idx_0 <= Controller_P.Saturation_LowerSat) {
 3213       rtb_Normalization_idx = Controller_P.Saturation_LowerSat;
 3214     } else {
 3215       rtb_Normalization_idx = rtb_Sum3_c_idx_0;
 3216     }
 3217   
 3218     if (rtb_Sum3_c_idx >= Controller_P.Saturation_UpperSat) {
 3219       omega_idx = Controller_P.Saturation_UpperSat;
 3220     } else if (rtb_Sum3_c_idx <= Controller_P.Saturation_LowerSat) {
 3221       omega_idx = Controller_P.Saturation_LowerSat;
 3222     } else {
 3223       omega_idx = rtb_Sum3_c_idx;
 3224     }
 3225   
 3226     /* Clock: '<S82>/Clock' */
 3227     rtb_Clock = Controller_M->Timing.t[0];
 3228   
 3229     /* MATLAB Function: '<S82>/MATLAB Function' incorporates:
 3230      *  Constant: '<S82>/Constant'
 3231      *  Constant: '<S82>/Constant1'
 3232      */
 3233     /* MATLAB Function 'Controll system/Observers/Hybrid nonlinear observer/MATLAB Function': '<S87>:1' */
 3234     /* '<S87>:1:5' if isempty(time_reset_x) || time < 1 */
 3235     if ((!Controller_DWork.time_reset_x_not_empty) || (rtb_Clock < 1.0)) {
 3236       /* '<S87>:1:6' time_reset_x = 0; */
 3237       Controller_DWork.time_reset_x = 0.0;
 3238       Controller_DWork.time_reset_x_not_empty = TRUE;
 3239     }
 3240   
 3241     /* '<S87>:1:10' if isempty(time_reset_y) || time < 1 */
 3242     if ((!Controller_DWork.time_reset_y_not_empty) || (rtb_Clock < 1.0)) {
 3243       /* '<S87>:1:11' time_reset_y = 0; */
 3244       Controller_DWork.time_reset_y = 0.0;
 3245       Controller_DWork.time_reset_y_not_empty = TRUE;
 3246     }
 3247   
 3248     /* '<S87>:1:15' if isempty(time_reset_psi) || time < 1 */
 3249     if ((!Controller_DWork.time_reset_psi_not_empty) || (rtb_Clock < 1.0)) {
 3250       /* '<S87>:1:16' time_reset_psi = 0; */
 3251       Controller_DWork.time_reset_psi = 0.0;
 3252       Controller_DWork.time_reset_psi_not_empty = TRUE;
 3253     }
 3254   
 3255     /* '<S87>:1:19' rst = [0 0 0]; */
 3256     Controller_B.rst[0] = 0.0;
 3257     Controller_B.rst[1] = 0.0;
 3258     Controller_B.rst[2] = 0.0;
 3259   
 3260     /* '<S87>:1:21' if abs(eta_tilde(1)) > limits(1) && time-time_reset_x > time_rst */
 3261     if ((fabs(rtb_Sum3_c_idx_1) > Controller_P.Constant_Value_l[0]) && (rtb_Clock
 3262          - Controller_DWork.time_reset_x > Controller_P.Constant1_Value_o)) {
 3263       /* '<S87>:1:22' rst(1) = 1; */
 3264       Controller_B.rst[0] = 1.0;
 3265   
 3266       /* '<S87>:1:23' time_reset_x = time; */
 3267       Controller_DWork.time_reset_x = rtb_Clock;
 3268     }
 3269   
 3270     /* '<S87>:1:28' if abs(eta_tilde(2)) > limits(2) && time-time_reset_y > time_rst */
 3271     if ((fabs(rtb_Sum3_c_idx_0) > Controller_P.Constant_Value_l[1]) && (rtb_Clock
 3272          - Controller_DWork.time_reset_y > Controller_P.Constant1_Value_o)) {
 3273       /* '<S87>:1:29' rst(2) = 1; */
 3274       Controller_B.rst[1] = 1.0;
 3275   
 3276       /* '<S87>:1:30' time_reset_y = time; */
 3277       Controller_DWork.time_reset_y = rtb_Clock;
 3278     }
 3279   
 3280     /* '<S87>:1:35' if abs(eta_tilde(3)) > limits(3) && time-time_reset_psi > time_rst */
 3281     if ((fabs(rtb_Sum3_c_idx) > Controller_P.Constant_Value_l[2]) && (rtb_Clock -
 3282          Controller_DWork.time_reset_psi > Controller_P.Constant1_Value_o)) {
 3283       /* '<S87>:1:36' rst(3) = 1; */
 3284       Controller_B.rst[2] = 1.0;
 3285   
 3286       /* '<S87>:1:37' time_reset_psi = time; */
 3287       Controller_DWork.time_reset_psi = rtb_Clock;
 3288     }
 3289   
 3290     /* End of MATLAB Function: '<S82>/MATLAB Function' */
 3291   
 3292     /* Clock: '<S92>/Clock' */
 3293     rtb_Clock_m = Controller_M->Timing.t[0];
 3294   
 3295     /* MATLAB Function: '<S92>/MATLAB Function' incorporates:
 3296      *  Constant: '<S92>/Constant'
 3297      *  Saturate: '<S92>/Saturation'
 3298      */
 3299     /* MATLAB Function 'Controll system/Observers/Hybrid nonlinear observer/Subsystem1/MATLAB Function': '<S102>:1' */
 3300     /* '<S102>:1:5' if isempty(nu_x) || time < 1 */
 3301     if ((!Controller_DWork.nu_x_not_empty) || (rtb_Clock_m < 1.0)) {
 3302       /* '<S102>:1:6' nu_x = 0; */
 3303       Controller_DWork.nu_x = 0.0;
 3304       Controller_DWork.nu_x_not_empty = TRUE;
 3305     }
 3306   
 3307     /* '<S102>:1:10' if isempty(nu_y) || time < 1 */
 3308     if ((!Controller_DWork.nu_y_not_empty) || (rtb_Clock_m < 1.0)) {
 3309       /* '<S102>:1:11' nu_y = 0; */
 3310       Controller_DWork.nu_y = 0.0;
 3311       Controller_DWork.nu_y_not_empty = TRUE;
 3312     }
 3313   
 3314     /* '<S102>:1:15' if isempty(nu_psi) || time < 1 */
 3315     if ((!Controller_DWork.nu_psi_not_empty) || (rtb_Clock_m < 1.0)) {
 3316       /* '<S102>:1:16' nu_psi = 0; */
 3317       Controller_DWork.nu_psi = 0.0;
 3318       Controller_DWork.nu_psi_not_empty = TRUE;
 3319     }
 3320   
 3321     /*  */
 3322     /* '<S102>:1:21' if rst(1) > 0.1 */
 3323     if (Controller_B.rst[0] > 0.1) {
 3324       /* '<S102>:1:22' nu_x = nu_x + gain(1)*eta_tilde(1); */
 3325       Controller_DWork.nu_x += Controller_P.Constant_Value_b[0] *
 3326         rtb_Normalization_idx_0;
 3327     }
 3328   
 3329     /* '<S102>:1:25' if rst(2) > 0.1 */
 3330     if (Controller_B.rst[1] > 0.1) {
 3331       /* '<S102>:1:26' nu_y = nu_y + gain(2)*eta_tilde(2); */
 3332       Controller_DWork.nu_y += Controller_P.Constant_Value_b[1] *
 3333         rtb_Normalization_idx;
 3334     }
 3335   
 3336     /* '<S102>:1:29' if rst(3) > 0.1 */
 3337     if (Controller_B.rst[2] > 0.1) {
 3338       /* '<S102>:1:30' nu_psi = nu_psi + gain(3)*eta_tilde(3); */
 3339       Controller_DWork.nu_psi += Controller_P.Constant_Value_b[2] * omega_idx;
 3340     }
 3341   
 3342     /* Sum: '<S92>/Sum' incorporates:
 3343      *  Gain: '<S92>/Gain'
 3344      *  Integrator: '<S92>/Integrator'
 3345      *  MATLAB Function: '<S92>/MATLAB Function'
 3346      */
 3347     /* '<S102>:1:33' d_hybrid = [nu_x nu_y nu_psi]'; */
 3348     Controller_B.Sum[0] = Controller_P.Gain_Gain_g * Controller_DWork.nu_x +
 3349       Controller_X.Integrator_CSTATE_d[0];
 3350     Controller_B.Sum[1] = Controller_P.Gain_Gain_g * Controller_DWork.nu_y +
 3351       Controller_X.Integrator_CSTATE_d[1];
 3352     Controller_B.Sum[2] = Controller_P.Gain_Gain_g * Controller_DWork.nu_psi +
 3353       Controller_X.Integrator_CSTATE_d[2];
 3354   
 3355     /* Gain: '<S82>/D' */
 3356     for (k = 0; k < 3; k++) {
 3357       rtb_sincos_o2[k] = Controller_P.D_Gain_l[k + 6] * Controller_B.Sum[2] +
 3358         (Controller_P.D_Gain_l[k + 3] * Controller_B.Sum[1] +
 3359          Controller_P.D_Gain_l[k] * Controller_B.Sum[0]);
 3360     }
 3361   
 3362     /* End of Gain: '<S82>/D' */
 3363   
 3364     /* SignalConversion: '<S94>/TmpSignal ConversionAtsincosInport1' */
 3365     rtb_frc_ctrl[0] = Controller_B.Gain3[5];
 3366     rtb_frc_ctrl[1] = Controller_B.Gain3[4];
 3367     rtb_frc_ctrl[2] = Controller_B.Gain3[3];
 3368   
 3369     /* Trigonometry: '<S97>/sincos' incorporates:
 3370      *  SignalConversion: '<S94>/TmpSignal ConversionAtsincosInport1'
 3371      */
 3372     rtb_Sum_g[0] = sin(Controller_B.Gain3[5]);
 3373     rtb_Gain6[0] = cos(Controller_B.Gain3[5]);
 3374     rtb_Sum_g[1] = sin(Controller_B.Gain3[4]);
 3375     rtb_Gain6[1] = cos(Controller_B.Gain3[4]);
 3376     rtb_Sum_g[2] = sin(Controller_B.Gain3[3]);
 3377     rtb_Gain6[2] = cos(Controller_B.Gain3[3]);
 3378   
 3379     /* Fcn: '<S97>/Fcn11' */
 3380     rtb_VectorConcatenate_nt[0] = rtb_Gain6[1] * rtb_Gain6[0];
 3381   
 3382     /* Fcn: '<S97>/Fcn21' */
 3383     rtb_VectorConcatenate_nt[1] = rtb_Sum_g[2] * rtb_Sum_g[1] * rtb_Gain6[0] -
 3384       rtb_Gain6[2] * rtb_Sum_g[0];
 3385   
 3386     /* Fcn: '<S97>/Fcn31' */
 3387     rtb_VectorConcatenate_nt[2] = rtb_Gain6[2] * rtb_Sum_g[1] * rtb_Gain6[0] +
 3388       rtb_Sum_g[2] * rtb_Sum_g[0];
 3389   
 3390     /* Fcn: '<S97>/Fcn12' */
 3391     rtb_VectorConcatenate_nt[3] = rtb_Gain6[1] * rtb_Sum_g[0];
 3392   
 3393     /* Fcn: '<S97>/Fcn22' */
 3394     rtb_VectorConcatenate_nt[4] = rtb_Sum_g[2] * rtb_Sum_g[1] * rtb_Sum_g[0] +
 3395       rtb_Gain6[2] * rtb_Gain6[0];
 3396   
 3397     /* Fcn: '<S97>/Fcn32' */
 3398     rtb_VectorConcatenate_nt[5] = rtb_Gain6[2] * rtb_Sum_g[1] * rtb_Sum_g[0] -
 3399       rtb_Sum_g[2] * rtb_Gain6[0];
 3400   
 3401     /* Fcn: '<S97>/Fcn13' */
 3402     rtb_VectorConcatenate_nt[6] = -rtb_Sum_g[1];
 3403   
 3404     /* Fcn: '<S97>/Fcn23' */
 3405     rtb_VectorConcatenate_nt[7] = rtb_Sum_g[2] * rtb_Gain6[1];
 3406   
 3407     /* Fcn: '<S97>/Fcn33' */
 3408     rtb_VectorConcatenate_nt[8] = rtb_Gain6[2] * rtb_Gain6[1];
 3409   
 3410     /* Gain: '<S82>/Gain6' */
 3411     for (k = 0; k < 3; k++) {
 3412       rtb_Gain6[k] = Controller_P.Gain6_Gain[k + 6] * rtb_Sum3_c_idx +
 3413         (Controller_P.Gain6_Gain[k + 3] * rtb_Sum3_c_idx_0 +
 3414          Controller_P.Gain6_Gain[k] * rtb_Sum3_c_idx_1);
 3415     }
 3416   
 3417     /* End of Gain: '<S82>/Gain6' */
 3418   
 3419     /* Saturate: '<S91>/Saturation' */
 3420     if (rtb_Gain6[0] >= Controller_P.Saturation_UpperSat_m) {
 3421       rtb_Normalization_idx_0 = Controller_P.Saturation_UpperSat_m;
 3422     } else if (rtb_Gain6[0] <= Controller_P.Saturation_LowerSat_m) {
 3423       rtb_Normalization_idx_0 = Controller_P.Saturation_LowerSat_m;
 3424     } else {
 3425       rtb_Normalization_idx_0 = rtb_Gain6[0];
 3426     }
 3427   
 3428     if (rtb_Gain6[1] >= Controller_P.Saturation_UpperSat_m) {
 3429       rtb_Normalization_idx = Controller_P.Saturation_UpperSat_m;
 3430     } else if (rtb_Gain6[1] <= Controller_P.Saturation_LowerSat_m) {
 3431       rtb_Normalization_idx = Controller_P.Saturation_LowerSat_m;
 3432     } else {
 3433       rtb_Normalization_idx = rtb_Gain6[1];
 3434     }
 3435   
 3436     if (rtb_Gain6[2] >= Controller_P.Saturation_UpperSat_m) {
 3437       omega_idx = Controller_P.Saturation_UpperSat_m;
 3438     } else if (rtb_Gain6[2] <= Controller_P.Saturation_LowerSat_m) {
 3439       omega_idx = Controller_P.Saturation_LowerSat_m;
 3440     } else {
 3441       omega_idx = rtb_Gain6[2];
 3442     }
 3443   
 3444     /* Clock: '<S91>/Clock' */
 3445     rtb_Clock_f = Controller_M->Timing.t[0];
 3446   
 3447     /* MATLAB Function: '<S91>/MATLAB Function' incorporates:
 3448      *  Constant: '<S91>/Constant1'
 3449      *  Saturate: '<S91>/Saturation'
 3450      */
 3451     /* MATLAB Function 'Controll system/Observers/Hybrid nonlinear observer/Subsystem/MATLAB Function': '<S101>:1' */
 3452     /* '<S101>:1:5' if isempty(d_x) || time < 1 */
 3453     if ((!Controller_DWork.d_x_not_empty) || (rtb_Clock_f < 1.0)) {
 3454       /* '<S101>:1:6' d_x = 0; */
 3455       Controller_DWork.d_x = 0.0;
 3456       Controller_DWork.d_x_not_empty = TRUE;
 3457     }
 3458   
 3459     /* '<S101>:1:10' if isempty(d_y) || time < 1 */
 3460     if ((!Controller_DWork.d_y_not_empty) || (rtb_Clock_f < 1.0)) {
 3461       /* '<S101>:1:11' d_y = 0; */
 3462       Controller_DWork.d_y = 0.0;
 3463       Controller_DWork.d_y_not_empty = TRUE;
 3464     }
 3465   
 3466     /* '<S101>:1:15' if isempty(d_psi) || time < 1 */
 3467     if ((!Controller_DWork.d_psi_not_empty) || (rtb_Clock_f < 1.0)) {
 3468       /* '<S101>:1:16' d_psi = 0; */
 3469       Controller_DWork.d_psi = 0.0;
 3470       Controller_DWork.d_psi_not_empty = TRUE;
 3471     }
 3472   
 3473     /* '<S101>:1:19' if rst(1) > 0.1 */
 3474     if (Controller_B.rst[0] > 0.1) {
 3475       /* '<S101>:1:20' d_x = d_x + gain(1)*eta_tilde(1); */
 3476       Controller_DWork.d_x += Controller_P.Constant1_Value_eh[0] *
 3477         rtb_Normalization_idx_0;
 3478     }
 3479   
 3480     /* '<S101>:1:23' if rst(2) > 0.1 */
 3481     if (Controller_B.rst[1] > 0.1) {
 3482       /* '<S101>:1:24' d_y = d_y + gain(2)*eta_tilde(2); */
 3483       Controller_DWork.d_y += Controller_P.Constant1_Value_eh[1] *
 3484         rtb_Normalization_idx;
 3485     }
 3486   
 3487     /* '<S101>:1:27' if rst(3) > 0.1 */
 3488     if (Controller_B.rst[2] > 0.1) {
 3489       /* '<S101>:1:28' d_psi = d_psi + gain(3)*eta_tilde(3); */
 3490       Controller_DWork.d_psi += Controller_P.Constant1_Value_eh[2] * omega_idx;
 3491     }
 3492   
 3493     /* Sum: '<S91>/Sum1' incorporates:
 3494      *  Gain: '<S91>/Gain'
 3495      *  Integrator: '<S91>/Integrator2'
 3496      *  MATLAB Function: '<S91>/MATLAB Function'
 3497      */
 3498     /* '<S101>:1:31' d_hybrid = [d_x d_y d_psi]'; */
 3499     rtb_Sum_g[0] = Controller_P.Gain_Gain_h * Controller_DWork.d_x +
 3500       Controller_X.Integrator2_CSTATE_n4[0];
 3501     rtb_Sum_g[1] = Controller_P.Gain_Gain_h * Controller_DWork.d_y +
 3502       Controller_X.Integrator2_CSTATE_n4[1];
 3503     rtb_Sum_g[2] = Controller_P.Gain_Gain_h * Controller_DWork.d_psi +
 3504       Controller_X.Integrator2_CSTATE_n4[2];
 3505   
 3506     /* Product: '<S89>/Product' incorporates:
 3507      *  Gain: '<S82>/Gain4'
 3508      */
 3509     for (k = 0; k < 3; k++) {
 3510       rtb_VectorConcatenate_p_0[k] = rtb_VectorConcatenate_nt[k + 6] * rtb_Sum_g[2]
 3511         + (rtb_VectorConcatenate_nt[k + 3] * rtb_Sum_g[1] +
 3512            rtb_VectorConcatenate_nt[k] * rtb_Sum_g[0]);
 3513     }
 3514   
 3515     /* End of Product: '<S89>/Product' */
 3516   
 3517     /* Gain: '<S82>/Gain4' */
 3518     for (k = 0; k < 3; k++) {
 3519       Controller_B.Gain4_k[k] = 0.0;
 3520       Controller_B.Gain4_k[k] += Controller_P.Gain4_Gain_h[k] *
 3521         rtb_VectorConcatenate_p_0[0];
 3522       Controller_B.Gain4_k[k] += Controller_P.Gain4_Gain_h[k + 3] *
 3523         rtb_VectorConcatenate_p_0[1];
 3524       Controller_B.Gain4_k[k] += Controller_P.Gain4_Gain_h[k + 6] *
 3525         rtb_VectorConcatenate_p_0[2];
 3526     }
 3527   
 3528     /* Gain: '<S13>/Gain6' incorporates:
 3529      *  Constant: '<S86>/Constant'
 3530      *  Sum: '<S82>/Sum4'
 3531      */
 3532     Controller_B.Gain6[0] = Controller_P.Gain6_Gain_d * Controller_B.Integrator1_h;
 3533     Controller_B.Gain6[1] = Controller_P.Gain6_Gain_d * Controller_B.Integrator2;
 3534     Controller_B.Gain6[2] = Controller_P.Gain6_Gain_d *
 3535       Controller_P.Constant_Value_e;
 3536     Controller_B.Gain6[3] = Controller_P.Gain6_Gain_d *
 3537       Controller_P.Constant_Value_e;
 3538     Controller_B.Gain6[4] = Controller_P.Gain6_Gain_d *
 3539       Controller_P.Constant_Value_e;
 3540     Controller_B.Gain6[5] = Controller_P.Gain6_Gain_d * Controller_B.Integrator3;
 3541     Controller_B.Gain6[6] = Controller_P.Gain6_Gain_d * Controller_B.Sum[0];
 3542     Controller_B.Gain6[7] = Controller_P.Gain6_Gain_d * Controller_B.Sum[1];
 3543     Controller_B.Gain6[8] = Controller_P.Gain6_Gain_d *
 3544       Controller_P.Constant_Value_e;
 3545     Controller_B.Gain6[9] = Controller_P.Gain6_Gain_d *
 3546       Controller_P.Constant_Value_e;
 3547     Controller_B.Gain6[10] = Controller_P.Gain6_Gain_d *
 3548       Controller_P.Constant_Value_e;
 3549     Controller_B.Gain6[11] = Controller_P.Gain6_Gain_d * Controller_B.Sum[2];
 3550     Controller_B.Gain6[12] = (Controller_B.Gain4_k[0] - rtb_sincos_o2[0]) *
 3551       Controller_P.Gain6_Gain_d;
 3552     Controller_B.Gain6[13] = (Controller_B.Gain4_k[1] - rtb_sincos_o2[1]) *
 3553       Controller_P.Gain6_Gain_d;
 3554     Controller_B.Gain6[14] = (Controller_B.Gain4_k[2] - rtb_sincos_o2[2]) *
 3555       Controller_P.Gain6_Gain_d;
 3556   
 3557     /* Outport: '<Root>/eta_e_N_HB' */
 3558     Controller_Y.eta_e_N_HB = Controller_B.Gain6[0];
 3559   
 3560     /* Outport: '<Root>/eta_e_E_HB' */
 3561     Controller_Y.eta_e_E_HB = Controller_B.Gain6[1];
 3562   
 3563     /* Outport: '<Root>/eta_e_psi_HB' */
 3564     Controller_Y.eta_e_psi_HB = Controller_B.Gain6[5];
 3565   
 3566     /* Outport: '<Root>/nu_e_u_HB' */
 3567     Controller_Y.nu_e_u_HB = Controller_B.Gain6[6];
 3568   
 3569     /* Outport: '<Root>/nu_e_v_HB' */
 3570     Controller_Y.nu_e_v_HB = Controller_B.Gain6[7];
 3571   
 3572     /* Outport: '<Root>/nu_e_r_HB' */
 3573     Controller_Y.nu_e_r_HB = Controller_B.Gain6[11];
 3574   
 3575     /* Outport: '<Root>/d_e_x_HB' */
 3576     Controller_Y.d_e_x_HB = Controller_B.Gain6[12];
 3577   
 3578     /* Outport: '<Root>/d_e_y_HB' */
 3579     Controller_Y.d_e_y_HB = Controller_B.Gain6[13];
 3580   
 3581     /* Outport: '<Root>/d_e_psi_HB' */
 3582     Controller_Y.d_e_psi_HB = Controller_B.Gain6[14];
 3583   
 3584     /* MATLAB Function: '<S16>/System parameter calculation' */
 3585     /* MATLAB Function 'Controll system/Control law/Robust MRAC/System parameter calculation': '<S24>:1' */
 3586     /* '<S24>:1:4' M = theta_v_hat_in(1:3,:); */
 3587     /* '<S24>:1:5' D = theta_v_hat_in(4:6,:); */
 3588     /* '<S24>:1:7' theta_v_hat_out = [diag(M)' diag(D)']'; */
 3589     for (k = 0; k < 3; k++) {
 3590       rtb_R_m[3 * k] = rtb_Integrator3[6 * k];
 3591       rtb_R_m[1 + 3 * k] = rtb_Integrator3[6 * k + 1];
 3592       rtb_R_m[2 + 3 * k] = rtb_Integrator3[6 * k + 2];
 3593     }
 3594   
 3595     for (k = 0; k < 3; k++) {
 3596       kp[3 * k] = rtb_Integrator3[6 * k + 3];
 3597       kp[1 + 3 * k] = rtb_Integrator3[6 * k + 4];
 3598       kp[2 + 3 * k] = rtb_Integrator3[6 * k + 5];
 3599     }
 3600   
 3601     /* Outport: '<Root>/M_v_x' incorporates:
 3602      *  MATLAB Function: '<S16>/System parameter calculation'
 3603      */
 3604     Controller_Y.M_v_x = rtb_R_m[0];
 3605   
 3606     /* Outport: '<Root>/M_v_y' incorporates:
 3607      *  MATLAB Function: '<S16>/System parameter calculation'
 3608      */
 3609     Controller_Y.M_v_y = rtb_R_m[4];
 3610   
 3611     /* Outport: '<Root>/M_v_n' incorporates:
 3612      *  MATLAB Function: '<S16>/System parameter calculation'
 3613      */
 3614     Controller_Y.M_v_n = rtb_R_m[8];
 3615   
 3616     /* Outport: '<Root>/D_v_x' incorporates:
 3617      *  MATLAB Function: '<S16>/System parameter calculation'
 3618      */
 3619     Controller_Y.D_v_x = kp[0];
 3620   
 3621     /* Outport: '<Root>/D_v_y' incorporates:
 3622      *  MATLAB Function: '<S16>/System parameter calculation'
 3623      */
 3624     Controller_Y.D_v_y = kp[4];
 3625   
 3626     /* Outport: '<Root>/D_v_n' incorporates:
 3627      *  MATLAB Function: '<S16>/System parameter calculation'
 3628      */
 3629     Controller_Y.D_v_n = kp[8];
 3630   
 3631     /* MATLAB Function: '<S16>/System parameter calculation1' */
 3632     /* MATLAB Function 'Controll system/Control law/Robust MRAC/System parameter calculation1': '<S25>:1' */
 3633     /* '<S25>:1:4' M_e = theta_e_hat_in(:,1:3); */
 3634     /* '<S25>:1:5' D_e = theta_e_hat_in(:,4:6); */
 3635     /* '<S25>:1:7' theta_e_hat_out = [diag(M_e)' diag(D_e)']'; */
 3636     for (k = 0; k < 3; k++) {
 3637       rtb_R_m[3 * k] = rtb_Integrator10[3 * k];
 3638       rtb_R_m[1 + 3 * k] = rtb_Integrator10[3 * k + 1];
 3639       rtb_R_m[2 + 3 * k] = rtb_Integrator10[3 * k + 2];
 3640     }
 3641   
 3642     for (k = 0; k < 3; k++) {
 3643       kp[3 * k] = rtb_Integrator10[(3 + k) * 3];
 3644       kp[1 + 3 * k] = rtb_Integrator10[(3 + k) * 3 + 1];
 3645       kp[2 + 3 * k] = rtb_Integrator10[(3 + k) * 3 + 2];
 3646     }
 3647   
 3648     /* Outport: '<Root>/D_e_x' incorporates:
 3649      *  MATLAB Function: '<S16>/System parameter calculation1'
 3650      */
 3651     Controller_Y.D_e_x = rtb_R_m[0];
 3652   
 3653     /* Outport: '<Root>/D_e_y' incorporates:
 3654      *  MATLAB Function: '<S16>/System parameter calculation1'
 3655      */
 3656     Controller_Y.D_e_y = rtb_R_m[4];
 3657   
 3658     /* Outport: '<Root>/D_e_n' incorporates:
 3659      *  MATLAB Function: '<S16>/System parameter calculation1'
 3660      */
 3661     Controller_Y.D_e_n = rtb_R_m[8];
 3662   
 3663     /* Outport: '<Root>/K_e_x' incorporates:
 3664      *  MATLAB Function: '<S16>/System parameter calculation1'
 3665      */
 3666     Controller_Y.K_e_x = kp[0];
 3667   
 3668     /* Outport: '<Root>/K_e_y' incorporates:
 3669      *  MATLAB Function: '<S16>/System parameter calculation1'
 3670      */
 3671     Controller_Y.K_e_y = kp[4];
 3672   
 3673     /* Outport: '<Root>/K_e_n' incorporates:
 3674      *  MATLAB Function: '<S16>/System parameter calculation1'
 3675      */
 3676     Controller_Y.K_e_n = kp[8];
 3677   
 3678     /* Gain: '<S16>/Gain' */
 3679     rtb_ypm[0] *= Controller_P.Gain_Gain_ix[0];
 3680     rtb_ypm[1] *= Controller_P.Gain_Gain_ix[1];
 3681   
 3682     /* Fcn: '<S23>/Fcn' */
 3683     rtb_Gain5_g = cos(rtb_TransferFcn2) * rtb_ypm[0] - sin(rtb_TransferFcn2) *
 3684       rtb_ypm[1];
 3685   
 3686     /* Outport: '<Root>/pref_x' */
 3687     Controller_Y.pref_x = rtb_Gain5_g;
 3688   
 3689     /* Fcn: '<S23>/Fcn1' */
 3690     rtb_Gain5_g = sin(rtb_TransferFcn2) * rtb_ypm[0] + cos(rtb_TransferFcn2) *
 3691       rtb_ypm[1];
 3692   
 3693     /* Outport: '<Root>/pref_y' */
 3694     Controller_Y.pref_y = rtb_Gain5_g;
 3695   
 3696     /* Fcn: '<S23>/Fcn2' incorporates:
 3697      *  Gain: '<S16>/Gain'
 3698      */
 3699     rtb_Gain5_g = Controller_P.Gain_Gain_ix[2] * rtb_ypm[2];
 3700   
 3701     /* Outport: '<Root>/pref_n' */
 3702     Controller_Y.pref_n = rtb_Gain5_g;
 3703   
 3704     /* Outport: '<Root>/fref_x' */
 3705     Controller_Y.fref_x = rtb_yFm[0];
 3706   
 3707     /* Outport: '<Root>/fref_y' */
 3708     Controller_Y.fref_y = rtb_yFm[1];
 3709   
 3710     /* Outport: '<Root>/fref_n' */
 3711     Controller_Y.fref_n = rtb_yFm[2];
 3712   
 3713     /* Outport: '<Root>/T_c_1x' incorporates:
 3714      *  S-Function (sdspmultiportsel): '<Root>/Multiport Selector'
 3715      */
 3716     Controller_Y.T_c_1x = rtb_theta_ctrl_hat[0];
 3717   
 3718     /* Outport: '<Root>/T_c_2x' incorporates:
 3719      *  S-Function (sdspmultiportsel): '<Root>/Multiport Selector'
 3720      */
 3721     Controller_Y.T_c_2x = rtb_theta_ctrl_hat[3];
 3722   
 3723     /* Outport: '<Root>/T_c_3x' incorporates:
 3724      *  MATLAB Function: '<S16>/Control law parameter calculation'
 3725      */
 3726     Controller_Y.T_c_3x = rtb_TmpSignalConversionAtSFun_n[0];
 3727   
 3728     /* Outport: '<Root>/C_c_0x' incorporates:
 3729      *  S-Function (sdspmultiportsel): '<Root>/Multiport Selector'
 3730      */
 3731     Controller_Y.C_c_0x = rtb_theta_ctrl_hat[9];
 3732   
 3733     /* Outport: '<Root>/T_c_4y' incorporates:
 3734      *  S-Function (sdspmultiportsel): '<Root>/Multiport Selector'
 3735      */
 3736     Controller_Y.T_c_4y = rtb_theta_ctrl_hat[1];
 3737   
 3738     /* Outport: '<Root>/T_c_5y' incorporates:
 3739      *  S-Function (sdspmultiportsel): '<Root>/Multiport Selector'
 3740      */
 3741     Controller_Y.T_c_5y = rtb_theta_ctrl_hat[4];
 3742   
 3743     /* Outport: '<Root>/T_c_6y' incorporates:
 3744      *  MATLAB Function: '<S16>/Control law parameter calculation'
 3745      */
 3746     Controller_Y.T_c_6y = rtb_TmpSignalConversionAtSFun_n[1];
 3747   
 3748     /* Outport: '<Root>/C_c_1y' incorporates:
 3749      *  S-Function (sdspmultiportsel): '<Root>/Multiport Selector'
 3750      */
 3751     Controller_Y.C_c_1y = rtb_theta_ctrl_hat[10];
 3752   
 3753     /* Outport: '<Root>/T_c_7n' incorporates:
 3754      *  S-Function (sdspmultiportsel): '<Root>/Multiport Selector'
 3755      */
 3756     Controller_Y.T_c_7n = rtb_theta_ctrl_hat[2];
 3757   
 3758     /* Outport: '<Root>/T_c_8n' incorporates:
 3759      *  S-Function (sdspmultiportsel): '<Root>/Multiport Selector'
 3760      */
 3761     Controller_Y.T_c_8n = rtb_theta_ctrl_hat[5];
 3762   
 3763     /* Outport: '<Root>/T_c_9n' incorporates:
 3764      *  MATLAB Function: '<S16>/Control law parameter calculation'
 3765      */
 3766     Controller_Y.T_c_9n = rtb_TmpSignalConversionAtSFun_n[2];
 3767   
 3768     /* Outport: '<Root>/C_c_2n' incorporates:
 3769      *  S-Function (sdspmultiportsel): '<Root>/Multiport Selector'
 3770      */
 3771     Controller_Y.C_c_2n = rtb_theta_ctrl_hat[11];
 3772   
 3773     /* Step: '<S11>/Step1' */
 3774     if (Controller_M->Timing.t[0] < Controller_P.Step1_Time_n) {
 3775       rtb_Gain5_g = Controller_P.Step1_Y0_e;
 3776     } else {
 3777       rtb_Gain5_g = Controller_P.Step1_YFinal_p;
 3778     }
 3779   
 3780     /* End of Step: '<S11>/Step1' */
 3781   
 3782     /* Sum: '<S16>/Sum' incorporates:
 3783      *  Constant: '<S16>/Ocean current  compensation'
 3784      *  Product: '<S11>/Product1'
 3785      */
 3786     rtb_TmpSignalConversionAtSFun_n[0] = rtb_Sum_be[0] * rtb_Gain5_g +
 3787       Controller_P.Oceancurrentcompensation_Value[0];
 3788     rtb_TmpSignalConversionAtSFun_n[1] = rtb_Sum_be[1] * rtb_Gain5_g +
 3789       Controller_P.Oceancurrentcompensation_Value[1];
 3790     rtb_TmpSignalConversionAtSFun_n[2] = Controller_B.Integrator[2] * rtb_Gain5_g
 3791       + Controller_P.Oceancurrentcompensation_Value[2];
 3792   
 3793     /* MATLAB Function: '<S16>/environment_estimation' incorporates:
 3794      *  Constant: '<S16>/Constant17'
 3795      *  Constant: '<S16>/Constant5'
 3796      *  Constant: '<S16>/Constant6'
 3797      *  Constant: '<S16>/Constant7'
 3798      *  Constant: '<S16>/Constant8'
 3799      *  Constant: '<S16>/Constant9'
 3800      *  Integrator: '<S16>/Integrator11'
 3801      *  Integrator: '<S16>/Integrator9'
 3802      */
 3803     /* MATLAB Function 'Controll system/Control law/Robust MRAC/environment_estimation': '<S29>:1' */
 3804     /* '<S29>:1:5' if isempty(first) */
 3805     /* Parameter estimates */
 3806     /* '<S29>:1:18' M_e = theta_hat(:,1:3); */
 3807     /* '<S29>:1:19' D_e = theta_hat(:,4:6); */
 3808     /* Initializing filter outputs */
 3809     /* '<S29>:1:22' z = zeros(3,1); */
 3810     /* '<S29>:1:23' phi1 = zeros(3,1); */
 3811     rtb_TmpSignalConversionAtSFun_g[0] = 0.0;
 3812     rtb_TmpSignalConversionAtSFun_g[1] = 0.0;
 3813     rtb_TmpSignalConversionAtSFun_g[2] = 0.0;
 3814   
 3815     /* '<S29>:1:24' phi2 = zeros(3,1); */
 3816     rtb_TmpSignalConversionAtSFu_gy[0] = 0.0;
 3817     rtb_TmpSignalConversionAtSFu_gy[1] = 0.0;
 3818     rtb_TmpSignalConversionAtSFu_gy[2] = 0.0;
 3819   
 3820     /* Filter states */
 3821     /* '<S29>:1:27' z1 = phi0(1:2,:); */
 3822     /* '<S29>:1:28' phi01 = phi0(3:4,:); */
 3823     /* '<S29>:1:29' phi02 = phi0(5:6,:); */
 3824     /* '<S29>:1:31' phi = zeros(3,1); */
 3825     /* Initializing derivative of theta estimate */
 3826     /* '<S29>:1:34' theta_hat_dot = zeros(3,6); */
 3827     memset(&Controller_B.theta_hat_dot[0], 0, 18U * sizeof(real_T));
 3828   
 3829     /* Initializing normalizing term derivative */
 3830     /* '<S29>:1:37' m_s_dot = zeros(3,1); */
 3831     /* '<S29>:1:39' for i=1:3 */
 3832     for (i = 0; i < 3; i++) {
 3833       /* '<S29>:1:41' z1_dot(:,i) = A01*z1(:,i)+B01*(F_C(i)); */
 3834       Controller_DWork.z1_dot[i << 1] = (Controller_X.Integrator9_CSTATE[6 * i + 1]
 3835         * Controller_P.Constant5_Value[2] + Controller_X.Integrator9_CSTATE[6 * i]
 3836         * Controller_P.Constant5_Value[0]) + Controller_P.Constant6_Value[0] *
 3837         Controller_B.Gain[i];
 3838       Controller_DWork.z1_dot[1 + (i << 1)] = (Controller_X.Integrator9_CSTATE[6 *
 3839         i + 1] * Controller_P.Constant5_Value[3] +
 3840         Controller_X.Integrator9_CSTATE[6 * i] * Controller_P.Constant5_Value[1])
 3841         + Controller_P.Constant6_Value[1] * Controller_B.Gain[i];
 3842   
 3843       /* '<S29>:1:42' z(i) = C01'*z1(:,i); */
 3844       rtb_TmpSignalConversionAtSFun_d[i] = Controller_X.Integrator9_CSTATE[6 * i +
 3845         1] * Controller_P.Constant7_Value[1] + Controller_X.Integrator9_CSTATE[6 *
 3846         i] * Controller_P.Constant7_Value[0];
 3847   
 3848       /* '<S29>:1:44' phi01_dot(:,i) = A11*phi01(:,i)+B11*nu_c(i); */
 3849       Controller_DWork.phi01_dot[i << 1] = (Controller_X.Integrator9_CSTATE[6 * i
 3850         + 2] * Controller_P.Constant8_Value_i[0] +
 3851         Controller_X.Integrator9_CSTATE[6 * i + 3] *
 3852         Controller_P.Constant8_Value_i[2]) + Controller_P.Constant9_Value[0] *
 3853         rtb_TmpSignalConversionAtSFun_n[i];
 3854       Controller_DWork.phi01_dot[1 + (i << 1)] = (Controller_X.Integrator9_CSTATE
 3855         [6 * i + 2] * Controller_P.Constant8_Value_i[1] +
 3856         Controller_X.Integrator9_CSTATE[6 * i + 3] *
 3857         Controller_P.Constant8_Value_i[3]) + Controller_P.Constant9_Value[1] *
 3858         rtb_TmpSignalConversionAtSFun_n[i];
 3859   
 3860       /* '<S29>:1:45' phi1(i) = C11'*phi01(:,i); */
 3861       rtb_Normalization_idx_0 = Controller_X.Integrator9_CSTATE[6 * i + 2] *
 3862         Controller_P.Constant17_Value[0] + Controller_X.Integrator9_CSTATE[6 * i +
 3863         3] * Controller_P.Constant17_Value[1];
 3864   
 3865       /* '<S29>:1:47' phi02_dot(:,i) = A01*phi02(:,i)+B01*nu_c(i); */
 3866       Controller_DWork.phi02_dot[i << 1] = (Controller_X.Integrator9_CSTATE[6 * i
 3867         + 4] * Controller_P.Constant5_Value[0] + Controller_X.Integrator9_CSTATE[6
 3868         * i + 5] * Controller_P.Constant5_Value[2]) +
 3869         Controller_P.Constant6_Value[0] * rtb_TmpSignalConversionAtSFun_n[i];
 3870       Controller_DWork.phi02_dot[1 + (i << 1)] = (Controller_X.Integrator9_CSTATE
 3871         [6 * i + 4] * Controller_P.Constant5_Value[1] +
 3872         Controller_X.Integrator9_CSTATE[6 * i + 5] * Controller_P.Constant5_Value
 3873         [3]) + Controller_P.Constant6_Value[1] * rtb_TmpSignalConversionAtSFun_n[i];
 3874   
 3875       /* '<S29>:1:48' phi2(i) = C01'*phi02(:,i); */
 3876       rtb_Normalization_idx = Controller_X.Integrator9_CSTATE[6 * i + 4] *
 3877         Controller_P.Constant7_Value[0] + Controller_X.Integrator9_CSTATE[6 * i +
 3878         5] * Controller_P.Constant7_Value[1];
 3879   
 3880       /* '<S29>:1:50' phi_n = [-phi1(i) -phi2(i)]'; */
 3881       /* m_s_dot(i) = -delta_0*m_s(i) + z(i)^2 + phi1(i)^2 + phi2(i)^2; */
 3882       /* '<S29>:1:53' m_s_dot(i) = -delta_0*m_s(i) + phi_n'*phi_n; */
 3883       Controller_B.m_s_dot_l[i] = (-rtb_Normalization_idx_0 *
 3884         -rtb_Normalization_idx_0 + -rtb_Normalization_idx * -rtb_Normalization_idx)
 3885         + -10.0 * Controller_X.Integrator11_CSTATE[i];
 3886       rtb_TmpSignalConversionAtSFun_g[i] = rtb_Normalization_idx_0;
 3887       rtb_TmpSignalConversionAtSFu_gy[i] = rtb_Normalization_idx;
 3888     }
 3889   
 3890     /* Adaptive gain */
 3891     /* '<S29>:1:58' Gamma = diag([700 200]); */
 3892     /* Upper bounds for leakage term */
 3893     /* '<S29>:1:61' M_0deu = [10000 10000 12000]'; */
 3894     /* '<S29>:1:62' M_0meu = [30000 30000 30000]'; */
 3895     /* Lower bounds for leakage term */
 3896     /* '<S29>:1:65' M_0del = [0 0 0]'; */
 3897     /* '<S29>:1:66' M_0mel = [10 10 10]'; */
 3898     /* '<S29>:1:68' w_0de = 1; */
 3899     /* '<S29>:1:69' w_0me = 1; */
 3900     /* '<S29>:1:71' for i=1:3 */
 3901     for (idx2 = 0; idx2 < 3; idx2++) {
 3902       /* Regressor */
 3903       /* '<S29>:1:74' phi = [-phi1(i) -phi2(i)]'; */
 3904       /* Parameter estimate */
 3905       /* '<S29>:1:77' theta_e_hat = [M_e(i,i) D_e(i,i)]'; */
 3906       /* Normalizing factor */
 3907       /* '<S29>:1:80' m = 1 + phi'*phi + m_s(i); */
 3908       rtb_Normalization_data[0] = -rtb_TmpSignalConversionAtSFun_g[idx2];
 3909       rtb_Normalization_data[1] = -rtb_TmpSignalConversionAtSFu_gy[idx2];
 3910       for (k = 0; k < 2; k++) {
 3911         for (i = 0; i < 1; i++) {
 3912           a_data[k] = rtb_Normalization_data[k];
 3913         }
 3914       }
 3915   
 3916       /* Estimate of error */
 3917       /* '<S29>:1:83' z_hat = theta_e_hat'*phi; */
 3918       /* '<S29>:1:84' eps = (z(i) - z_hat)/m; */
 3919       rtb_Sum3_g = (rtb_TmpSignalConversionAtSFun_d[idx2] - (rtb_Integrator10[(3 +
 3920         idx2) * 3 + idx2] * -rtb_TmpSignalConversionAtSFu_gy[idx2] +
 3921         rtb_Integrator10[3 * idx2 + idx2] * -rtb_TmpSignalConversionAtSFun_g[idx2]))
 3922         / (((a_data[0] * -rtb_TmpSignalConversionAtSFun_g[idx2] + a_data[1] *
 3923              -rtb_TmpSignalConversionAtSFu_gy[idx2]) + 1.0) +
 3924            Controller_X.Integrator11_CSTATE[idx2]);
 3925   
 3926       /* Switching leakage for M_e */
 3927       /* '<S29>:1:87' if and(M_0mel(i) < M_e(i,i), M_e(i,i) < M_0meu(i)) */
 3928       if ((10.0 < rtb_Integrator10[3 * idx2 + idx2]) && (rtb_Integrator10[3 * idx2
 3929            + idx2] < 30000.0)) {
 3930         /* '<S29>:1:88' w_sme = 0; */
 3931         rtb_Divide = 0.0;
 3932       } else if (((30000.0 <= rtb_Integrator10[3 * idx2 + idx2]) &&
 3933                   (rtb_Integrator10[3 * idx2 + idx2] <= 60000.0)) || ((5.0 <=
 3934                    rtb_Integrator10[3 * idx2 + idx2]) && (rtb_Integrator10[3 *
 3935                    idx2 + idx2] <= 10.0))) {
 3936         /* '<S29>:1:89' elseif or(and(M_0meu(i) <= M_e(i,i), M_e(i,i) <= 2*M_0meu(i)), and(M_0mel(i)/2 <= M_e(i,i), M_e(i,i) <= M_0mel(i))) */
 3937         /* '<S29>:1:90' w_sme = (abs(M_e(i,i))/M_0meu(i) - 1)*w_0me; */
 3938         rtb_Divide = fabs(rtb_Integrator10[3 * idx2 + idx2]) / 30000.0 - 1.0;
 3939       } else {
 3940         /* '<S29>:1:91' else */
 3941         /* '<S29>:1:92' w_sme = w_0me; */
 3942         rtb_Divide = 1.0;
 3943       }
 3944   
 3945       /* Switching leakage for D_e */
 3946       /* '<S29>:1:96' if and(M_0del(i) < D_e(i,i), D_e(i,i) < M_0deu(i)) */
 3947       if ((0.0 < rtb_Integrator10[(3 + idx2) * 3 + idx2]) && (rtb_Integrator10[(3
 3948             + idx2) * 3 + idx2] < (real_T)M_0deu[idx2])) {
 3949         /* '<S29>:1:97' w_sde = 0; */
 3950         rtb_Sum2_i = 0.0;
 3951       } else if ((((real_T)M_0deu[idx2] <= rtb_Integrator10[(3 + idx2) * 3 + idx2])
 3952                   && (rtb_Integrator10[(3 + idx2) * 3 + idx2] <= 2.0 * (real_T)
 3953                       M_0deu[idx2])) || ((0.0 <= rtb_Integrator10[(3 + idx2) * 3 +
 3954         idx2]) && (rtb_Integrator10[(3 + idx2) * 3 + idx2] <= 0.0))) {
 3955         /* '<S29>:1:98' elseif or(and(M_0deu(i) <= D_e(i,i), D_e(i,i) <= 2*M_0deu(i)), and(M_0del(i)/2 <= D_e(i,i), D_e(i,i) <= M_0del(i))) */
 3956         /* '<S29>:1:99' w_sde = (abs(D_e(i,i))/M_0deu(i) - 1)*w_0de; */
 3957         rtb_Sum2_i = fabs(rtb_Integrator10[(3 + idx2) * 3 + idx2]) / (real_T)
 3958           M_0deu[idx2] - 1.0;
 3959       } else {
 3960         /* '<S29>:1:100' else */
 3961         /* '<S29>:1:101' w_sde = w_0de; */
 3962         rtb_Sum2_i = 1.0;
 3963       }
 3964   
 3965       /* '<S29>:1:104' W_se = diag([w_sme w_sde]); */
 3966       /* '<S29>:1:107' theta_hat_dot(i,i:3:end) = Gamma*eps*phi - Gamma*W_se*theta_e_hat; */
 3967       for (k = 0; k < 2; k++) {
 3968         diophantine_eq[k] = 0.0;
 3969         diophantine_eq[k] += (real_T)b_a[k] * rtb_Divide;
 3970         diophantine_eq[k + 2] = 0.0;
 3971         diophantine_eq[k + 2] += (real_T)b_a[k + 2] * rtb_Sum2_i;
 3972       }
 3973   
 3974       Controller_B.theta_hat_dot[idx2 + 3 * idx2] = (700.0 * rtb_Sum3_g *
 3975         -rtb_TmpSignalConversionAtSFun_g[idx2] + 0.0 * rtb_Sum3_g *
 3976         -rtb_TmpSignalConversionAtSFu_gy[idx2]) - (rtb_Integrator10[(3 + idx2) * 3
 3977         + idx2] * diophantine_eq[2] + rtb_Integrator10[3 * idx2 + idx2] *
 3978         diophantine_eq[0]);
 3979       Controller_B.theta_hat_dot[idx2 + 3 * (idx2 + 3)] = (0.0 * rtb_Sum3_g *
 3980         -rtb_TmpSignalConversionAtSFun_g[idx2] + 200.0 * rtb_Sum3_g *
 3981         -rtb_TmpSignalConversionAtSFu_gy[idx2]) - (rtb_Integrator10[(3 + idx2) * 3
 3982         + idx2] * diophantine_eq[3] + rtb_Integrator10[3 * idx2 + idx2] *
 3983         diophantine_eq[1]);
 3984     }
 3985   
 3986     /* '<S29>:1:111' phi_dot = [z1_dot; phi01_dot; phi02_dot]; */
 3987     /* '<S29>:1:112' theta_e_hat = [M_e; D_e]; */
 3988     for (k = 0; k < 3; k++) {
 3989       Controller_B.phi_dot_e[6 * k] = Controller_DWork.z1_dot[k << 1];
 3990       Controller_B.phi_dot_e[1 + 6 * k] = Controller_DWork.z1_dot[(k << 1) + 1];
 3991     }
 3992   
 3993     for (k = 0; k < 3; k++) {
 3994       Controller_B.phi_dot_e[2 + 6 * k] = Controller_DWork.phi01_dot[k << 1];
 3995       Controller_B.phi_dot_e[3 + 6 * k] = Controller_DWork.phi01_dot[(k << 1) + 1];
 3996     }
 3997   
 3998     for (k = 0; k < 3; k++) {
 3999       Controller_B.phi_dot_e[4 + 6 * k] = Controller_DWork.phi02_dot[k << 1];
 4000       Controller_B.phi_dot_e[5 + 6 * k] = Controller_DWork.phi02_dot[(k << 1) + 1];
 4001     }
 4002   
 4003     /* End of MATLAB Function: '<S16>/environment_estimation' */
 4004   
 4005     /* SignalConversion: '<S30>/TmpSignal ConversionAt SFunction Inport10' incorporates:
 4006      *  MATLAB Function: '<S16>/normalized _adaptive_law1'
 4007      */
 4008     rtb_TmpSignalConversionAtSFun_g[0] = rtb_Row1;
 4009     rtb_TmpSignalConversionAtSFun_g[1] = rtb_Row2;
 4010     rtb_TmpSignalConversionAtSFun_g[2] = rtb_Row3;
 4011   
 4012     /* MATLAB Function: '<S16>/normalized _adaptive_law1' incorporates:
 4013      *  Constant: '<S16>/Constant1'
 4014      *  Constant: '<S16>/Constant11'
 4015      *  Constant: '<S16>/Constant12'
 4016      *  Constant: '<S16>/Constant2'
 4017      *  Constant: '<S16>/Constant3'
 4018      *  Constant: '<S16>/Constant4'
 4019      *  Integrator: '<S16>/Integrator4'
 4020      *  Integrator: '<S16>/Integrator5'
 4021      */
 4022     /* MATLAB Function 'Controll system/Control law/Robust MRAC/normalized _adaptive_law1': '<S30>:1' */
 4023     /* '<S30>:1:5' if isempty(first) */
 4024     /*  Initializing parameter derivative */
 4025     /* '<S30>:1:32' theta_ves_hat_dot = zeros(6,3); */
 4026     memset(&rtb_Integrator10[0], 0, 18U * sizeof(real_T));
 4027   
 4028     /*  Parameter estimates */
 4029     /* '<S30>:1:35' M = theta_ves_hat(1:3,:); */
 4030     /* '<S30>:1:36' D = theta_ves_hat(4:6,:); */
 4031     /* '<S30>:1:38' kp = M\eye(3); */
 4032     /* '<S30>:1:39' p2 = kp*D; */
 4033     /*  Filter states */
 4034     /* '<S30>:1:42' phi01=phi(1:2,:); */
 4035     /* '<S30>:1:43' phi02=phi(3:4,:); */
 4036     /* '<S30>:1:44' phi03=phi(5:6,:); */
 4037     /* '<S30>:1:45' phi04=phi(7:8,:); */
 4038     /* '<S30>:1:46' phi05=phi(9:10,:); */
 4039     /* Initializing normalizing term derivative */
 4040     /* '<S30>:1:49' m_s_dot = zeros(3,1); */
 4041     /* '<S30>:1:51' for i=1:3 */
 4042     for (i = 0; i < 3; i++) {
 4043       /*  sF_T/Lambda(s) */
 4044       /* '<S30>:1:54' dphi01(:,i) = A11*phi01(:,i)+B11*F_T(i); */
 4045       Controller_DWork.dphi01[i << 1] = (Controller_X.Integrator4_CSTATE[10 * i +
 4046         1] * Controller_P.Constant4_Value_p[2] + Controller_X.Integrator4_CSTATE
 4047         [10 * i] * Controller_P.Constant4_Value_p[0]) +
 4048         Controller_P.Constant3_Value_f[0] * rtb_Z_m[i];
 4049       Controller_DWork.dphi01[1 + (i << 1)] = (Controller_X.Integrator4_CSTATE[10 *
 4050         i + 1] * Controller_P.Constant4_Value_p[3] +
 4051         Controller_X.Integrator4_CSTATE[10 * i] * Controller_P.Constant4_Value_p[1])
 4052         + Controller_P.Constant3_Value_f[1] * rtb_Z_m[i];
 4053   
 4054       /* '<S30>:1:55' phi1(i) = C11'*phi01(:,i); */
 4055       Controller_DWork.phi1[i] = Controller_X.Integrator4_CSTATE[10 * i + 1] *
 4056         Controller_P.Constant2_Value_g[1] + Controller_X.Integrator4_CSTATE[10 * i]
 4057         * Controller_P.Constant2_Value_g[0];
 4058   
 4059       /*  F_T/Lambda(s) */
 4060       /* '<S30>:1:58' dphi02(:,i) = A01*phi02(:,i)+B01*F_T(i); */
 4061       Controller_DWork.dphi02[i << 1] = (Controller_X.Integrator4_CSTATE[10 * i +
 4062         2] * Controller_P.Constant1_Value_o3[0] + Controller_X.Integrator4_CSTATE
 4063         [10 * i + 3] * Controller_P.Constant1_Value_o3[2]) +
 4064         Controller_P.Constant12_Value[0] * rtb_Z_m[i];
 4065       Controller_DWork.dphi02[1 + (i << 1)] = (Controller_X.Integrator4_CSTATE[10 *
 4066         i + 2] * Controller_P.Constant1_Value_o3[1] +
 4067         Controller_X.Integrator4_CSTATE[10 * i + 3] *
 4068         Controller_P.Constant1_Value_o3[3]) + Controller_P.Constant12_Value[1] *
 4069         rtb_Z_m[i];
 4070   
 4071       /* '<S30>:1:59' phi2(i) = C01'*phi02(:,i); */
 4072       Controller_DWork.phi2[i] = Controller_X.Integrator4_CSTATE[10 * i + 2] *
 4073         Controller_P.Constant11_Value[0] + Controller_X.Integrator4_CSTATE[10 * i
 4074         + 3] * Controller_P.Constant11_Value[1];
 4075   
 4076       /*  nu/Lambda(s) */
 4077       /* '<S30>:1:62' dphi03(:,i) = A01*phi03(:,i)+B01*nu(i); */
 4078       Controller_DWork.dphi03[i << 1] = (Controller_X.Integrator4_CSTATE[10 * i +
 4079         4] * Controller_P.Constant1_Value_o3[0] + Controller_X.Integrator4_CSTATE
 4080         [10 * i + 5] * Controller_P.Constant1_Value_o3[2]) +
 4081         Controller_P.Constant12_Value[0] * rtb_TmpSignalConversionAtSFun_n[i];
 4082       Controller_DWork.dphi03[1 + (i << 1)] = (Controller_X.Integrator4_CSTATE[10 *
 4083         i + 4] * Controller_P.Constant1_Value_o3[1] +
 4084         Controller_X.Integrator4_CSTATE[10 * i + 5] *
 4085         Controller_P.Constant1_Value_o3[3]) + Controller_P.Constant12_Value[1] *
 4086         rtb_TmpSignalConversionAtSFun_n[i];
 4087   
 4088       /* '<S30>:1:63' phi3(i) = C01'*phi03(:,i); */
 4089       Controller_DWork.phi3[i] = Controller_X.Integrator4_CSTATE[10 * i + 4] *
 4090         Controller_P.Constant11_Value[0] + Controller_X.Integrator4_CSTATE[10 * i
 4091         + 5] * Controller_P.Constant11_Value[1];
 4092   
 4093       /*  eta_p/Lambda(s) */
 4094       /* '<S30>:1:66' dphi04(:,i) = A01*phi04(:,i)+B01*eta_p(i); */
 4095       Controller_DWork.dphi04[i << 1] = (Controller_X.Integrator4_CSTATE[10 * i +
 4096         6] * Controller_P.Constant1_Value_o3[0] + Controller_X.Integrator4_CSTATE
 4097         [10 * i + 7] * Controller_P.Constant1_Value_o3[2]) +
 4098         Controller_P.Constant12_Value[0] * rtb_TmpSignalConversionAtSFun_g[i];
 4099       Controller_DWork.dphi04[1 + (i << 1)] = (Controller_X.Integrator4_CSTATE[10 *
 4100         i + 6] * Controller_P.Constant1_Value_o3[1] +
 4101         Controller_X.Integrator4_CSTATE[10 * i + 7] *
 4102         Controller_P.Constant1_Value_o3[3]) + Controller_P.Constant12_Value[1] *
 4103         rtb_TmpSignalConversionAtSFun_g[i];
 4104   
 4105       /* '<S30>:1:67' phi4(i) = C01'*phi04(:,i); */
 4106       /*  snu/Lambda(s) */
 4107       /* '<S30>:1:70' dphi05(:,i) = A11*phi05(:,i)+B11*nu(i); */
 4108       Controller_DWork.dphi05[i << 1] = (Controller_X.Integrator4_CSTATE[10 * i +
 4109         8] * Controller_P.Constant4_Value_p[0] + Controller_X.Integrator4_CSTATE
 4110         [10 * i + 9] * Controller_P.Constant4_Value_p[2]) +
 4111         Controller_P.Constant3_Value_f[0] * rtb_TmpSignalConversionAtSFun_n[i];
 4112       Controller_DWork.dphi05[1 + (i << 1)] = (Controller_X.Integrator4_CSTATE[10 *
 4113         i + 8] * Controller_P.Constant4_Value_p[1] +
 4114         Controller_X.Integrator4_CSTATE[10 * i + 9] *
 4115         Controller_P.Constant4_Value_p[3]) + Controller_P.Constant3_Value_f[1] *
 4116         rtb_TmpSignalConversionAtSFun_n[i];
 4117   
 4118       /* '<S30>:1:71' phi5(i) = C11'*phi05(:,i); */
 4119       Controller_DWork.phi5[i] = Controller_X.Integrator4_CSTATE[10 * i + 8] *
 4120         Controller_P.Constant2_Value_g[0] + Controller_X.Integrator4_CSTATE[10 * i
 4121         + 9] * Controller_P.Constant2_Value_g[1];
 4122   
 4123       /* '<S30>:1:73' phi_1(i,:) = [phi1(i) phi2(i)]; */
 4124       /* '<S30>:1:74' phi_2(i,:) = [-phi3(i) -phi5(i)]; */
 4125       /* Dynamic normalization */
 4126       /* m_s_dot(i) = -delta_0*m_s(i) + phi2(i)*phi2(i)' + phi_2(i,:)*phi_2(i,:)'; */
 4127       /* '<S30>:1:78' m_s_dot(i) = -delta_0*m_s(i) + F_T(i)^2 + nu(i)^2; */
 4128       Controller_B.m_s_dot[i] = (-10.0 * Controller_X.Integrator5_CSTATE[i] +
 4129         rt_powd_snf(rtb_Z_m[i], 2.0)) + rt_powd_snf
 4130         (rtb_TmpSignalConversionAtSFun_n[i], 2.0);
 4131     }
 4132   
 4133     /* Filter vectors */
 4134     /* '<S30>:1:82' phi_0 = phi_1; */
 4135     /* '<S30>:1:83' phi_1bar = zeros(3,1); */
 4136     /* '<S30>:1:84' phi_1m = phi_0(:,2); */
 4137     /* Adaption gain */
 4138     /* '<S30>:1:87' Gamma = diag([6100 200]); */
 4139     /* Upper bounds for switching term */
 4140     /* '<S30>:1:90' M_0du = [10000 10000 12000]'; */
 4141     /* '<S30>:1:91' M_0mu = [30000 30000 30000]'; */
 4142     /* Lower bounds for switching term */
 4143     /* '<S30>:1:94' M_0dl = [0 0 0]'; */
 4144     /* '<S30>:1:95' M_0ml = [200 200 200]'; */
 4145     /* '<S30>:1:97' w_0d = 1; */
 4146     /* '<S30>:1:98' w_0m = 1; */
 4147     /* '<S30>:1:100' for i=1:3 */
 4148     for (idx2 = 0; idx2 < 3; idx2++) {
 4149       /* Parameter estimate and regressor */
 4150       /* '<S30>:1:103' theta_p_hat = [M(i,i) D(i,i)]'; */
 4151       /* '<S30>:1:104' phi = [phi5(i) phi3(i)]'; */
 4152       /* Measurement and estimate of this */
 4153       /* '<S30>:1:107' z = phi2(i); */
 4154       /* '<S30>:1:108' z_hat = theta_p_hat'*phi; */
 4155       /*  Normalizing factor */
 4156       /* '<S30>:1:111' m = 1 + m_s(i); */
 4157       /* Estimation error */
 4158       /* '<S30>:1:114' eps = (z - z_hat)/m; */
 4159       rtb_Sum3_g = (Controller_DWork.phi2[idx2] - (rtb_Integrator3[(6 * idx2 +
 4160         idx2) + 3] * Controller_DWork.phi3[idx2] + rtb_Integrator3[6 * idx2 + idx2]
 4161         * Controller_DWork.phi5[idx2])) / (1.0 +
 4162         Controller_X.Integrator5_CSTATE[idx2]);
 4163   
 4164       /* Switching leakage for M */
 4165       /* '<S30>:1:117' if and(M_0ml(i) < M(i,i), M(i,i) < M_0mu(i)) */
 4166       if ((200.0 < rtb_Integrator3[6 * idx2 + idx2]) && (rtb_Integrator3[6 * idx2
 4167            + idx2] < 30000.0)) {
 4168         /* '<S30>:1:118' w_sm = 0; */
 4169         rtb_Divide = 0.0;
 4170       } else if (((30000.0 <= rtb_Integrator3[6 * idx2 + idx2]) &&
 4171                   (rtb_Integrator3[6 * idx2 + idx2] <= 60000.0)) || ((100.0 <=
 4172                    rtb_Integrator3[6 * idx2 + idx2]) && (rtb_Integrator3[6 * idx2
 4173                    + idx2] <= 200.0))) {
 4174         /* '<S30>:1:119' elseif or(and(M_0mu(i) <= M(i,i), M(i,i) <= 2*M_0mu(i)), and(M_0ml(i)/2 <= M(i,i), M(i,i) <= M_0ml(i))) */
 4175         /* '<S30>:1:120' w_sm = (abs(M(i,i))/M_0mu(i) - 1)*w_0m; */
 4176         rtb_Divide = fabs(rtb_Integrator3[6 * idx2 + idx2]) / 30000.0 - 1.0;
 4177       } else {
 4178         /* '<S30>:1:121' else */
 4179         /* '<S30>:1:122' w_sm = w_0m; */
 4180         rtb_Divide = 1.0;
 4181       }
 4182   
 4183       /* Switching leakage for D */
 4184       /* '<S30>:1:126' if and(M_0dl(i) < D(i,i), D(i,i) < M_0du(i)) */
 4185       if ((0.0 < rtb_Integrator3[(6 * idx2 + idx2) + 3]) && (rtb_Integrator3[(6 *
 4186             idx2 + idx2) + 3] < (real_T)M_0du[idx2])) {
 4187         /* '<S30>:1:127' w_sd = 0; */
 4188         rtb_Sum2_i = 0.0;
 4189       } else if ((((real_T)M_0du[idx2] <= rtb_Integrator3[(6 * idx2 + idx2) + 3]) &&
 4190                   (rtb_Integrator3[(6 * idx2 + idx2) + 3] <= 2.0 * (real_T)
 4191                    M_0du[idx2])) || ((0.0 <= rtb_Integrator3[(6 * idx2 + idx2) + 3])
 4192                   && (rtb_Integrator3[(6 * idx2 + idx2) + 3] <= 0.0))) {
 4193         /* '<S30>:1:128' elseif or(and(M_0du(i) <= D(i,i), D(i,i) <= 2*M_0du(i)), and(M_0dl(i)/2 <= D(i,i), D(i,i) <= M_0dl(i))) */
 4194         /* '<S30>:1:129' w_sd = (abs(D(i,i))/M_0du(i) - 1)*w_0d; */
 4195         rtb_Sum2_i = fabs(rtb_Integrator3[(6 * idx2 + idx2) + 3]) / (real_T)
 4196           M_0du[idx2] - 1.0;
 4197       } else {
 4198         /* '<S30>:1:130' else */
 4199         /* '<S30>:1:131' w_sd = w_0d; */
 4200         rtb_Sum2_i = 1.0;
 4201       }
 4202   
 4203       /* '<S30>:1:134' W_s = diag([w_sm w_sd]); */
 4204       /* Update law */
 4205       /* '<S30>:1:137' theta_ves_hat_dot(i:3:end,i) = Gamma*phi*eps - Gamma*W_s*theta_p_hat; */
 4206       for (k = 0; k < 2; k++) {
 4207         diophantine_eq[k] = 0.0;
 4208         diophantine_eq[k] += (real_T)a[k] * rtb_Divide;
 4209         diophantine_eq[k + 2] = 0.0;
 4210         diophantine_eq[k + 2] += (real_T)a[k + 2] * rtb_Sum2_i;
 4211       }
 4212   
 4213       rtb_Integrator10[idx2 + 6 * idx2] = (6100.0 * Controller_DWork.phi5[idx2] +
 4214         0.0 * Controller_DWork.phi3[idx2]) * rtb_Sum3_g - (rtb_Integrator3[(6 *
 4215         idx2 + idx2) + 3] * diophantine_eq[2] + rtb_Integrator3[6 * idx2 + idx2] *
 4216         diophantine_eq[0]);
 4217       rtb_Integrator10[(idx2 + 6 * idx2) + 3] = (0.0 * Controller_DWork.phi5[idx2]
 4218         + 200.0 * Controller_DWork.phi3[idx2]) * rtb_Sum3_g - (rtb_Integrator3[(6 *
 4219         idx2 + idx2) + 3] * diophantine_eq[3] + rtb_Integrator3[6 * idx2 + idx2] *
 4220         diophantine_eq[1]);
 4221     }
 4222   
 4223     /*  Stack the derivatives. */
 4224     /* '<S30>:1:142' phi_dot = [dphi01; dphi02; dphi03; dphi04; dphi05]; */
 4225     for (k = 0; k < 18; k++) {
 4226       Controller_B.theta_ves_hat_dot[k] = rtb_Integrator10[k];
 4227     }
 4228   
 4229     for (k = 0; k < 3; k++) {
 4230       Controller_B.phi_dot[10 * k] = Controller_DWork.dphi01[k << 1];
 4231       Controller_B.phi_dot[1 + 10 * k] = Controller_DWork.dphi01[(k << 1) + 1];
 4232     }
 4233   
 4234     for (k = 0; k < 3; k++) {
 4235       Controller_B.phi_dot[2 + 10 * k] = Controller_DWork.dphi02[k << 1];
 4236       Controller_B.phi_dot[3 + 10 * k] = Controller_DWork.dphi02[(k << 1) + 1];
 4237     }
 4238   
 4239     for (k = 0; k < 3; k++) {
 4240       Controller_B.phi_dot[4 + 10 * k] = Controller_DWork.dphi03[k << 1];
 4241       Controller_B.phi_dot[5 + 10 * k] = Controller_DWork.dphi03[(k << 1) + 1];
 4242     }
 4243   
 4244     for (k = 0; k < 3; k++) {
 4245       Controller_B.phi_dot[6 + 10 * k] = Controller_DWork.dphi04[k << 1];
 4246       Controller_B.phi_dot[7 + 10 * k] = Controller_DWork.dphi04[(k << 1) + 1];
 4247     }
 4248   
 4249     for (k = 0; k < 3; k++) {
 4250       Controller_B.phi_dot[8 + 10 * k] = Controller_DWork.dphi05[k << 1];
 4251       Controller_B.phi_dot[9 + 10 * k] = Controller_DWork.dphi05[(k << 1) + 1];
 4252     }
 4253   
 4254     /* Gain: '<S33>/Gain1' incorporates:
 4255      *  Constant: '<S33>/Constant'
 4256      *  Gain: '<S33>/Gain'
 4257      *  Gain: '<S33>/Gain2'
 4258      *  Sum: '<S33>/Sum'
 4259      *  Sum: '<S33>/Sum1'
 4260      *  Sum: '<S33>/Sum2'
 4261      *  Trigonometry: '<S36>/Trigonometric Function'
 4262      */
 4263     Controller_B.Gain1 = (((rt_atan2d_snf(rtb_TransferFcn2_h, rtb_Sum_k) -
 4264       Controller_P.Constant_Value_p) * Controller_P.Gain2_Gain_m -
 4265       rtb_Integrator1_i) - Controller_P.Gain_Gain_bs * Controller_B.Integrator_n) *
 4266       Controller_P.Gain1_Gain_f;
 4267   
 4268     /* Trigonometry: '<S47>/sincos' */
 4269     rtb_ypm[0] = sin(rtb_k_m_r[0]);
 4270     rtb_Sum_g[0] = cos(rtb_k_m_r[0]);
 4271     rtb_ypm[1] = sin(rtb_k_m_r[1]);
 4272     rtb_Sum_g[1] = cos(rtb_k_m_r[1]);
 4273     rtb_ypm[2] = sin(rtb_k_m_r[2]);
 4274     rtb_Sum_g[2] = cos(rtb_k_m_r[2]);
 4275   
 4276     /* Fcn: '<S47>/Fcn11' */
 4277     rtb_VectorConcatenate_g[0] = rtb_Sum_g[1] * rtb_Sum_g[0];
 4278   
 4279     /* Fcn: '<S47>/Fcn21' */
 4280     rtb_VectorConcatenate_g[1] = rtb_ypm[2] * rtb_ypm[1] * rtb_Sum_g[0] -
 4281       rtb_Sum_g[2] * rtb_ypm[0];
 4282   
 4283     /* Fcn: '<S47>/Fcn31' */
 4284     rtb_VectorConcatenate_g[2] = rtb_Sum_g[2] * rtb_ypm[1] * rtb_Sum_g[0] +
 4285       rtb_ypm[2] * rtb_ypm[0];
 4286   
 4287     /* Fcn: '<S47>/Fcn12' */
 4288     rtb_VectorConcatenate_g[3] = rtb_Sum_g[1] * rtb_ypm[0];
 4289   
 4290     /* Fcn: '<S47>/Fcn22' */
 4291     rtb_VectorConcatenate_g[4] = rtb_ypm[2] * rtb_ypm[1] * rtb_ypm[0] + rtb_Sum_g
 4292       [2] * rtb_Sum_g[0];
 4293   
 4294     /* Fcn: '<S47>/Fcn32' */
 4295     rtb_VectorConcatenate_g[5] = rtb_Sum_g[2] * rtb_ypm[1] * rtb_ypm[0] - rtb_ypm
 4296       [2] * rtb_Sum_g[0];
 4297   
 4298     /* Fcn: '<S47>/Fcn13' */
 4299     rtb_VectorConcatenate_g[6] = -rtb_ypm[1];
 4300   
 4301     /* Fcn: '<S47>/Fcn23' */
 4302     rtb_VectorConcatenate_g[7] = rtb_ypm[2] * rtb_Sum_g[1];
 4303   
 4304     /* Fcn: '<S47>/Fcn33' */
 4305     rtb_VectorConcatenate_g[8] = rtb_Sum_g[2] * rtb_Sum_g[1];
 4306   
 4307     /* Sum: '<S43>/Sum1' incorporates:
 4308      *  Constant: '<S43>/Gravity'
 4309      *  Inport: '<Root>/w_d'
 4310      */
 4311     rtb_Gain5_g = Controller_U.w_d + Controller_P.Gravity_Value_n;
 4312   
 4313     /* Product: '<S43>/Product' incorporates:
 4314      *  Inport: '<Root>/u_d'
 4315      *  Inport: '<Root>/v_d'
 4316      *  SignalConversion: '<S43>/TmpSignal ConversionAtProductInport2'
 4317      */
 4318     for (k = 0; k < 3; k++) {
 4319       rtb_Sum_g[k] = rtb_VectorConcatenate_g[k + 6] * rtb_Gain5_g +
 4320         (rtb_VectorConcatenate_g[k + 3] * Controller_U.v_d +
 4321          rtb_VectorConcatenate_g[k] * Controller_U.u_d);
 4322     }
 4323   
 4324     /* End of Product: '<S43>/Product' */
 4325   
 4326     /* Trigonometry: '<S48>/sincos' */
 4327     rtb_ypm[0] = sin(rtb_k_m_r[0]);
 4328     rtb_Sum_be[0] = cos(rtb_k_m_r[0]);
 4329     rtb_ypm[1] = sin(rtb_k_m_r[1]);
 4330     rtb_Sum_be[1] = cos(rtb_k_m_r[1]);
 4331     rtb_ypm[2] = sin(rtb_k_m_r[2]);
 4332     rtb_Sum_be[2] = cos(rtb_k_m_r[2]);
 4333   
 4334     /* Fcn: '<S48>/Fcn11' */
 4335     rtb_VectorConcatenate_pj[0] = rtb_Sum_be[1] * rtb_Sum_be[0];
 4336   
 4337     /* Fcn: '<S48>/Fcn21' */
 4338     rtb_VectorConcatenate_pj[1] = rtb_ypm[2] * rtb_ypm[1] * rtb_Sum_be[0] -
 4339       rtb_Sum_be[2] * rtb_ypm[0];
 4340   
 4341     /* Fcn: '<S48>/Fcn31' */
 4342     rtb_VectorConcatenate_pj[2] = rtb_Sum_be[2] * rtb_ypm[1] * rtb_Sum_be[0] +
 4343       rtb_ypm[2] * rtb_ypm[0];
 4344   
 4345     /* Fcn: '<S48>/Fcn12' */
 4346     rtb_VectorConcatenate_pj[3] = rtb_Sum_be[1] * rtb_ypm[0];
 4347   
 4348     /* Fcn: '<S48>/Fcn22' */
 4349     rtb_VectorConcatenate_pj[4] = rtb_ypm[2] * rtb_ypm[1] * rtb_ypm[0] +
 4350       rtb_Sum_be[2] * rtb_Sum_be[0];
 4351   
 4352     /* Fcn: '<S48>/Fcn32' */
 4353     rtb_VectorConcatenate_pj[5] = rtb_Sum_be[2] * rtb_ypm[1] * rtb_ypm[0] -
 4354       rtb_ypm[2] * rtb_Sum_be[0];
 4355   
 4356     /* Fcn: '<S48>/Fcn13' */
 4357     rtb_VectorConcatenate_pj[6] = -rtb_ypm[1];
 4358   
 4359     /* Fcn: '<S48>/Fcn23' */
 4360     rtb_VectorConcatenate_pj[7] = rtb_ypm[2] * rtb_Sum_be[1];
 4361   
 4362     /* Fcn: '<S48>/Fcn33' */
 4363     rtb_VectorConcatenate_pj[8] = rtb_Sum_be[2] * rtb_Sum_be[1];
 4364   
 4365     /* Product: '<S43>/Product1' */
 4366     for (k = 0; k < 3; k++) {
 4367       rtb_ypm[k] = rtb_VectorConcatenate_pj[k + 6] * rtb_Gain_ia +
 4368         (rtb_VectorConcatenate_pj[k + 3] * rtb_Gain1_f +
 4369          rtb_VectorConcatenate_pj[k] * rtb_Sum1_pd_idx);
 4370     }
 4371   
 4372     /* End of Product: '<S43>/Product1' */
 4373   
 4374     /* Sum: '<S55>/Sum' incorporates:
 4375      *  Constant: '<S44>/Accelerometer position'
 4376      *  Product: '<S55>/Product'
 4377      *  Product: '<S55>/Product1'
 4378      */
 4379     rtb_TmpSignalConversionAtSFun_g[0] = rtb_ypm[1] *
 4380       Controller_P.Accelerometerposition_Value[2] - rtb_ypm[2] *
 4381       Controller_P.Accelerometerposition_Value[1];
 4382     rtb_TmpSignalConversionAtSFun_g[1] = rtb_ypm[2] *
 4383       Controller_P.Accelerometerposition_Value[0] - rtb_ypm[0] *
 4384       Controller_P.Accelerometerposition_Value[2];
 4385     rtb_TmpSignalConversionAtSFun_g[2] = rtb_ypm[0] *
 4386       Controller_P.Accelerometerposition_Value[1] - rtb_ypm[1] *
 4387       Controller_P.Accelerometerposition_Value[0];
 4388   
 4389     /* Trigonometry: '<S79>/sincos' */
 4390     rtb_Sum1_pd_idx = sin(rtb_k_m_r[0]);
 4391     rtb_Sum_be[0] = cos(rtb_k_m_r[0]);
 4392     rtb_Gain1_f = sin(rtb_k_m_r[1]);
 4393     rtb_Sum_be[1] = cos(rtb_k_m_r[1]);
 4394     rtb_Gain_ia = sin(rtb_k_m_r[2]);
 4395     rtb_Sum_be[2] = cos(rtb_k_m_r[2]);
 4396   
 4397     /* Fcn: '<S79>/Fcn11' */
 4398     rtb_VectorConcatenate_a[0] = rtb_Sum_be[1] * rtb_Sum_be[0];
 4399   
 4400     /* Fcn: '<S79>/Fcn21' */
 4401     rtb_VectorConcatenate_a[1] = rtb_Gain_ia * rtb_Gain1_f * rtb_Sum_be[0] -
 4402       rtb_Sum_be[2] * rtb_Sum1_pd_idx;
 4403   
 4404     /* Fcn: '<S79>/Fcn31' */
 4405     rtb_VectorConcatenate_a[2] = rtb_Sum_be[2] * rtb_Gain1_f * rtb_Sum_be[0] +
 4406       rtb_Gain_ia * rtb_Sum1_pd_idx;
 4407   
 4408     /* Fcn: '<S79>/Fcn12' */
 4409     rtb_VectorConcatenate_a[3] = rtb_Sum_be[1] * rtb_Sum1_pd_idx;
 4410   
 4411     /* Fcn: '<S79>/Fcn22' */
 4412     rtb_VectorConcatenate_a[4] = rtb_Gain_ia * rtb_Gain1_f * rtb_Sum1_pd_idx +
 4413       rtb_Sum_be[2] * rtb_Sum_be[0];
 4414   
 4415     /* Fcn: '<S79>/Fcn32' */
 4416     rtb_VectorConcatenate_a[5] = rtb_Sum_be[2] * rtb_Gain1_f * rtb_Sum1_pd_idx -
 4417       rtb_Gain_ia * rtb_Sum_be[0];
 4418   
 4419     /* Fcn: '<S79>/Fcn13' */
 4420     rtb_VectorConcatenate_a[6] = -rtb_Gain1_f;
 4421   
 4422     /* Fcn: '<S79>/Fcn23' */
 4423     rtb_VectorConcatenate_a[7] = rtb_Gain_ia * rtb_Sum_be[1];
 4424   
 4425     /* Fcn: '<S79>/Fcn33' */
 4426     rtb_VectorConcatenate_a[8] = rtb_Sum_be[2] * rtb_Sum_be[1];
 4427   
 4428     /* Product: '<S46>/Product2' */
 4429     for (k = 0; k < 3; k++) {
 4430       Controller_B.Product2[k] = 0.0;
 4431       Controller_B.Product2[k] += rtb_VectorConcatenate_a[k] * rtb_Gain1_l_idx_1;
 4432       Controller_B.Product2[k] += rtb_VectorConcatenate_a[k + 3] *
 4433         rtb_Gain1_l_idx_0;
 4434       Controller_B.Product2[k] += rtb_VectorConcatenate_a[k + 6] * rtb_Gain1_l_idx;
 4435     }
 4436   
 4437     /* End of Product: '<S46>/Product2' */
 4438   
 4439     /* Sum: '<S57>/Sum' incorporates:
 4440      *  Constant: '<S44>/Accelerometer position'
 4441      *  Product: '<S57>/Product'
 4442      *  Product: '<S57>/Product1'
 4443      */
 4444     rtb_Sum1_pd_idx = Controller_B.Product2[1] *
 4445       Controller_P.Accelerometerposition_Value[2] - Controller_B.Product2[2] *
 4446       Controller_P.Accelerometerposition_Value[1];
 4447     rtb_Gain1_f = Controller_B.Product2[2] *
 4448       Controller_P.Accelerometerposition_Value[0] - Controller_B.Product2[0] *
 4449       Controller_P.Accelerometerposition_Value[2];
 4450     rtb_Gain_ia = Controller_B.Product2[0] *
 4451       Controller_P.Accelerometerposition_Value[1] - Controller_B.Product2[1] *
 4452       Controller_P.Accelerometerposition_Value[0];
 4453   
 4454     /* Product: '<S56>/Product' */
 4455     rtb_Gain1_l_idx_1 = Controller_B.Product2[1] * rtb_Gain_ia;
 4456     rtb_Gain1_l_idx_0 = Controller_B.Product2[2] * rtb_Sum1_pd_idx;
 4457     rtb_Gain1_l_idx = Controller_B.Product2[0] * rtb_Gain1_f;
 4458   
 4459     /* Product: '<S56>/Product1' */
 4460     rtb_Sum_be[0] = Controller_B.Product2[2] * rtb_Gain1_f;
 4461     rtb_Sum_be[1] = Controller_B.Product2[0] * rtb_Gain_ia;
 4462     rtb_Sum_be[2] = Controller_B.Product2[1] * rtb_Sum1_pd_idx;
 4463     if (rtmIsMajorTimeStep(Controller_M)) {
 4464       /* Gain: '<S58>/Output' incorporates:
 4465        *  RandomNumber: '<S58>/White Noise'
 4466        */
 4467       Controller_B.Output = Controller_P.Output_Gain * Controller_DWork.NextOutput;
 4468     }
 4469   
 4470     /* TransferFcn: '<S59>/Transfer Fcn' */
 4471     rtb_Gain5_g = Controller_P.TransferFcn_C_n[0]*
 4472       Controller_X.TransferFcn_CSTATE_en[0]
 4473       + Controller_P.TransferFcn_C_n[1]*Controller_X.TransferFcn_CSTATE_en[1];
 4474   
 4475     /* TransferFcn: '<S59>/Transfer Fcn1' */
 4476     rtb_TransferFcn1_o = Controller_P.TransferFcn1_C_e[0]*
 4477       Controller_X.TransferFcn1_CSTATE_m[0]
 4478       + Controller_P.TransferFcn1_C_e[1]*Controller_X.TransferFcn1_CSTATE_m[1];
 4479   
 4480     /* TransferFcn: '<S59>/Transfer Fcn2' */
 4481     rtb_TransferFcn2_h = Controller_P.TransferFcn2_C_c[0]*
 4482       Controller_X.TransferFcn2_CSTATE_j[0]
 4483       + Controller_P.TransferFcn2_C_c[1]*Controller_X.TransferFcn2_CSTATE_j[1];
 4484   
 4485     /* Sum: '<S51>/Sum' incorporates:
 4486      *  Sum: '<S56>/Sum'
 4487      */
 4488     Controller_B.Sum_a[0] = (rtb_TmpSignalConversionAtSFun_g[0] + rtb_Sum_g[0]) +
 4489       (rtb_Gain1_l_idx_1 - rtb_Sum_be[0]);
 4490     Controller_B.Sum_a[1] = (rtb_TmpSignalConversionAtSFun_g[1] + rtb_Sum_g[1]) +
 4491       (rtb_Gain1_l_idx_0 - rtb_Sum_be[1]);
 4492     Controller_B.Sum_a[2] = (rtb_TmpSignalConversionAtSFun_g[2] + rtb_Sum_g[2]) +
 4493       (rtb_Gain1_l_idx - rtb_Sum_be[2]);
 4494   
 4495     /* Sum: '<S51>/Sum1' */
 4496     rtb_Sum1_pd_idx = rtb_Gain5_g + Controller_B.Output;
 4497     rtb_Gain1_f = rtb_TransferFcn1_o + Controller_B.Output;
 4498     rtb_Gain_ia = rtb_TransferFcn2_h + Controller_B.Output;
 4499   
 4500     /* Sum: '<S60>/Sum' incorporates:
 4501      *  Constant: '<S44>/Accelerometer position1'
 4502      *  Product: '<S60>/Product'
 4503      *  Product: '<S60>/Product1'
 4504      */
 4505     rtb_TmpSignalConversionAtSFun_g[0] = rtb_ypm[1] *
 4506       Controller_P.Accelerometerposition1_Value[2] - rtb_ypm[2] *
 4507       Controller_P.Accelerometerposition1_Value[1];
 4508     rtb_TmpSignalConversionAtSFun_g[1] = rtb_ypm[2] *
 4509       Controller_P.Accelerometerposition1_Value[0] - rtb_ypm[0] *
 4510       Controller_P.Accelerometerposition1_Value[2];
 4511     rtb_TmpSignalConversionAtSFun_g[2] = rtb_ypm[0] *
 4512       Controller_P.Accelerometerposition1_Value[1] - rtb_ypm[1] *
 4513       Controller_P.Accelerometerposition1_Value[0];
 4514   
 4515     /* Sum: '<S62>/Sum' incorporates:
 4516      *  Constant: '<S44>/Accelerometer position1'
 4517      *  Product: '<S62>/Product'
 4518      *  Product: '<S62>/Product1'
 4519      */
 4520     rtb_Gain1_l_idx_1 = Controller_B.Product2[1] *
 4521       Controller_P.Accelerometerposition1_Value[2] - Controller_B.Product2[2] *
 4522       Controller_P.Accelerometerposition1_Value[1];
 4523     rtb_Gain1_l_idx_0 = Controller_B.Product2[2] *
 4524       Controller_P.Accelerometerposition1_Value[0] - Controller_B.Product2[0] *
 4525       Controller_P.Accelerometerposition1_Value[2];
 4526     rtb_Gain1_l_idx = Controller_B.Product2[0] *
 4527       Controller_P.Accelerometerposition1_Value[1] - Controller_B.Product2[1] *
 4528       Controller_P.Accelerometerposition1_Value[0];
 4529   
 4530     /* Product: '<S61>/Product' */
 4531     rtb_Sum_be[0] = Controller_B.Product2[1] * rtb_Gain1_l_idx;
 4532     rtb_Sum_be[1] = Controller_B.Product2[2] * rtb_Gain1_l_idx_1;
 4533     rtb_Sum_be[2] = Controller_B.Product2[0] * rtb_Gain1_l_idx_0;
 4534   
 4535     /* Product: '<S61>/Product1' */
 4536     rtb_TmpSignalConversionAtSFun_d[0] = Controller_B.Product2[2] *
 4537       rtb_Gain1_l_idx_0;
 4538     rtb_TmpSignalConversionAtSFun_d[1] = Controller_B.Product2[0] *
 4539       rtb_Gain1_l_idx;
 4540     rtb_TmpSignalConversionAtSFun_d[2] = Controller_B.Product2[1] *
 4541       rtb_Gain1_l_idx_1;
 4542     if (rtmIsMajorTimeStep(Controller_M)) {
 4543       /* Gain: '<S63>/Output' incorporates:
 4544        *  RandomNumber: '<S63>/White Noise'
 4545        */
 4546       Controller_B.Output_g = Controller_P.Output_Gain_b *
 4547         Controller_DWork.NextOutput_k;
 4548     }
 4549   
 4550     /* TransferFcn: '<S64>/Transfer Fcn' */
 4551     rtb_Gain5_g = Controller_P.TransferFcn_C_c[0]*
 4552       Controller_X.TransferFcn_CSTATE_d[0]
 4553       + Controller_P.TransferFcn_C_c[1]*Controller_X.TransferFcn_CSTATE_d[1];
 4554   
 4555     /* TransferFcn: '<S64>/Transfer Fcn1' */
 4556     rtb_TransferFcn1_o = Controller_P.TransferFcn1_C_o2[0]*
 4557       Controller_X.TransferFcn1_CSTATE_e[0]
 4558       + Controller_P.TransferFcn1_C_o2[1]*Controller_X.TransferFcn1_CSTATE_e[1];
 4559   
 4560     /* TransferFcn: '<S64>/Transfer Fcn2' */
 4561     rtb_TransferFcn2_h = Controller_P.TransferFcn2_C_e[0]*
 4562       Controller_X.TransferFcn2_CSTATE_c[0]
 4563       + Controller_P.TransferFcn2_C_e[1]*Controller_X.TransferFcn2_CSTATE_c[1];
 4564   
 4565     /* Sum: '<S52>/Sum' incorporates:
 4566      *  Sum: '<S61>/Sum'
 4567      */
 4568     Controller_B.Sum_f[0] = (rtb_TmpSignalConversionAtSFun_g[0] + rtb_Sum_g[0]) +
 4569       (rtb_Sum_be[0] - rtb_TmpSignalConversionAtSFun_d[0]);
 4570     Controller_B.Sum_f[1] = (rtb_TmpSignalConversionAtSFun_g[1] + rtb_Sum_g[1]) +
 4571       (rtb_Sum_be[1] - rtb_TmpSignalConversionAtSFun_d[1]);
 4572     Controller_B.Sum_f[2] = (rtb_TmpSignalConversionAtSFun_g[2] + rtb_Sum_g[2]) +
 4573       (rtb_Sum_be[2] - rtb_TmpSignalConversionAtSFun_d[2]);
 4574   
 4575     /* Sum: '<S52>/Sum1' */
 4576     rtb_TmpSignalConversionAtSFun_d[0] = rtb_Gain5_g + Controller_B.Output_g;
 4577     rtb_TmpSignalConversionAtSFun_d[1] = rtb_TransferFcn1_o +
 4578       Controller_B.Output_g;
 4579     rtb_TmpSignalConversionAtSFun_d[2] = rtb_TransferFcn2_h +
 4580       Controller_B.Output_g;
 4581   
 4582     /* Sum: '<S65>/Sum' incorporates:
 4583      *  Constant: '<S44>/Accelerometer position3'
 4584      *  Product: '<S65>/Product'
 4585      *  Product: '<S65>/Product1'
 4586      */
 4587     rtb_TmpSignalConversionAtSFun_g[0] = rtb_ypm[1] *
 4588       Controller_P.Accelerometerposition3_Value[2] - rtb_ypm[2] *
 4589       Controller_P.Accelerometerposition3_Value[1];
 4590     rtb_TmpSignalConversionAtSFun_g[1] = rtb_ypm[2] *
 4591       Controller_P.Accelerometerposition3_Value[0] - rtb_ypm[0] *
 4592       Controller_P.Accelerometerposition3_Value[2];
 4593     rtb_TmpSignalConversionAtSFun_g[2] = rtb_ypm[0] *
 4594       Controller_P.Accelerometerposition3_Value[1] - rtb_ypm[1] *
 4595       Controller_P.Accelerometerposition3_Value[0];
 4596   
 4597     /* Sum: '<S67>/Sum' incorporates:
 4598      *  Constant: '<S44>/Accelerometer position3'
 4599      *  Product: '<S67>/Product'
 4600      *  Product: '<S67>/Product1'
 4601      */
 4602     rtb_Gain1_l_idx_1 = Controller_B.Product2[1] *
 4603       Controller_P.Accelerometerposition3_Value[2] - Controller_B.Product2[2] *
 4604       Controller_P.Accelerometerposition3_Value[1];
 4605     rtb_Gain1_l_idx_0 = Controller_B.Product2[2] *
 4606       Controller_P.Accelerometerposition3_Value[0] - Controller_B.Product2[0] *
 4607       Controller_P.Accelerometerposition3_Value[2];
 4608     rtb_Gain1_l_idx = Controller_B.Product2[0] *
 4609       Controller_P.Accelerometerposition3_Value[1] - Controller_B.Product2[1] *
 4610       Controller_P.Accelerometerposition3_Value[0];
 4611   
 4612     /* Product: '<S66>/Product' */
 4613     rtb_Sum_be[0] = Controller_B.Product2[1] * rtb_Gain1_l_idx;
 4614     rtb_Sum_be[1] = Controller_B.Product2[2] * rtb_Gain1_l_idx_1;
 4615     rtb_Sum_be[2] = Controller_B.Product2[0] * rtb_Gain1_l_idx_0;
 4616   
 4617     /* Product: '<S66>/Product1' */
 4618     rtb_TmpSignalConversionAtSFu_gy[0] = Controller_B.Product2[2] *
 4619       rtb_Gain1_l_idx_0;
 4620     rtb_TmpSignalConversionAtSFu_gy[1] = Controller_B.Product2[0] *
 4621       rtb_Gain1_l_idx;
 4622     rtb_TmpSignalConversionAtSFu_gy[2] = Controller_B.Product2[1] *
 4623       rtb_Gain1_l_idx_1;
 4624     if (rtmIsMajorTimeStep(Controller_M)) {
 4625       /* Gain: '<S68>/Output' incorporates:
 4626        *  RandomNumber: '<S68>/White Noise'
 4627        */
 4628       Controller_B.Output_c = Controller_P.Output_Gain_g *
 4629         Controller_DWork.NextOutput_f;
 4630     }
 4631   
 4632     /* TransferFcn: '<S69>/Transfer Fcn' */
 4633     rtb_Gain5_g = Controller_P.TransferFcn_C_e[0]*
 4634       Controller_X.TransferFcn_CSTATE_o[0]
 4635       + Controller_P.TransferFcn_C_e[1]*Controller_X.TransferFcn_CSTATE_o[1];
 4636   
 4637     /* TransferFcn: '<S69>/Transfer Fcn1' */
 4638     rtb_TransferFcn1_o = Controller_P.TransferFcn1_C_m[0]*
 4639       Controller_X.TransferFcn1_CSTATE_g[0]
 4640       + Controller_P.TransferFcn1_C_m[1]*Controller_X.TransferFcn1_CSTATE_g[1];
 4641   
 4642     /* TransferFcn: '<S69>/Transfer Fcn2' */
 4643     rtb_TransferFcn2_h = Controller_P.TransferFcn2_C_m[0]*
 4644       Controller_X.TransferFcn2_CSTATE_i[0]
 4645       + Controller_P.TransferFcn2_C_m[1]*Controller_X.TransferFcn2_CSTATE_i[1];
 4646   
 4647     /* Sum: '<S53>/Sum' incorporates:
 4648      *  Sum: '<S66>/Sum'
 4649      */
 4650     Controller_B.Sum_l[0] = (rtb_TmpSignalConversionAtSFun_g[0] + rtb_Sum_g[0]) +
 4651       (rtb_Sum_be[0] - rtb_TmpSignalConversionAtSFu_gy[0]);
 4652     Controller_B.Sum_l[1] = (rtb_TmpSignalConversionAtSFun_g[1] + rtb_Sum_g[1]) +
 4653       (rtb_Sum_be[1] - rtb_TmpSignalConversionAtSFu_gy[1]);
 4654     Controller_B.Sum_l[2] = (rtb_TmpSignalConversionAtSFun_g[2] + rtb_Sum_g[2]) +
 4655       (rtb_Sum_be[2] - rtb_TmpSignalConversionAtSFu_gy[2]);
 4656   
 4657     /* Sum: '<S53>/Sum1' */
 4658     rtb_TmpSignalConversionAtSFu_gy[0] = rtb_Gain5_g + Controller_B.Output_c;
 4659     rtb_TmpSignalConversionAtSFu_gy[1] = rtb_TransferFcn1_o +
 4660       Controller_B.Output_c;
 4661     rtb_TmpSignalConversionAtSFu_gy[2] = rtb_TransferFcn2_h +
 4662       Controller_B.Output_c;
 4663   
 4664     /* Sum: '<S70>/Sum' incorporates:
 4665      *  Constant: '<S44>/Accelerometer position2'
 4666      *  Product: '<S70>/Product'
 4667      *  Product: '<S70>/Product1'
 4668      */
 4669     rtb_TmpSignalConversionAtSFun_g[0] = rtb_ypm[1] *
 4670       Controller_P.Accelerometerposition2_Value[2] - rtb_ypm[2] *
 4671       Controller_P.Accelerometerposition2_Value[1];
 4672     rtb_TmpSignalConversionAtSFun_g[1] = rtb_ypm[2] *
 4673       Controller_P.Accelerometerposition2_Value[0] - rtb_ypm[0] *
 4674       Controller_P.Accelerometerposition2_Value[2];
 4675     rtb_TmpSignalConversionAtSFun_g[2] = rtb_ypm[0] *
 4676       Controller_P.Accelerometerposition2_Value[1] - rtb_ypm[1] *
 4677       Controller_P.Accelerometerposition2_Value[0];
 4678   
 4679     /* Sum: '<S72>/Sum' incorporates:
 4680      *  Constant: '<S44>/Accelerometer position2'
 4681      *  Product: '<S72>/Product'
 4682      *  Product: '<S72>/Product1'
 4683      */
 4684     rtb_ypm[0] = Controller_B.Product2[1] *
 4685       Controller_P.Accelerometerposition2_Value[2] - Controller_B.Product2[2] *
 4686       Controller_P.Accelerometerposition2_Value[1];
 4687     rtb_ypm[1] = Controller_B.Product2[2] *
 4688       Controller_P.Accelerometerposition2_Value[0] - Controller_B.Product2[0] *
 4689       Controller_P.Accelerometerposition2_Value[2];
 4690     rtb_ypm[2] = Controller_B.Product2[0] *
 4691       Controller_P.Accelerometerposition2_Value[1] - Controller_B.Product2[1] *
 4692       Controller_P.Accelerometerposition2_Value[0];
 4693   
 4694     /* Product: '<S71>/Product' */
 4695     rtb_Gain1_l_idx_1 = Controller_B.Product2[1] * rtb_ypm[2];
 4696     rtb_Gain1_l_idx_0 = Controller_B.Product2[2] * rtb_ypm[0];
 4697     rtb_Gain1_l_idx = Controller_B.Product2[0] * rtb_ypm[1];
 4698   
 4699     /* Product: '<S71>/Product1' */
 4700     rtb_Sum_be[0] = Controller_B.Product2[2] * rtb_ypm[1];
 4701     rtb_Sum_be[1] = Controller_B.Product2[0] * rtb_ypm[2];
 4702     rtb_Sum_be[2] = Controller_B.Product2[1] * rtb_ypm[0];
 4703     if (rtmIsMajorTimeStep(Controller_M)) {
 4704       /* Gain: '<S73>/Output' incorporates:
 4705        *  RandomNumber: '<S73>/White Noise'
 4706        */
 4707       Controller_B.Output_o = Controller_P.Output_Gain_e *
 4708         Controller_DWork.NextOutput_c;
 4709     }
 4710   
 4711     /* TransferFcn: '<S74>/Transfer Fcn' */
 4712     rtb_Gain5_g = Controller_P.TransferFcn_C_dx[0]*
 4713       Controller_X.TransferFcn_CSTATE_c[0]
 4714       + Controller_P.TransferFcn_C_dx[1]*Controller_X.TransferFcn_CSTATE_c[1];
 4715   
 4716     /* TransferFcn: '<S74>/Transfer Fcn1' */
 4717     rtb_TransferFcn1_o = Controller_P.TransferFcn1_C_e2[0]*
 4718       Controller_X.TransferFcn1_CSTATE_l[0]
 4719       + Controller_P.TransferFcn1_C_e2[1]*Controller_X.TransferFcn1_CSTATE_l[1];
 4720   
 4721     /* TransferFcn: '<S74>/Transfer Fcn2' */
 4722     rtb_TransferFcn2_h = Controller_P.TransferFcn2_C_h[0]*
 4723       Controller_X.TransferFcn2_CSTATE_h[0]
 4724       + Controller_P.TransferFcn2_C_h[1]*Controller_X.TransferFcn2_CSTATE_h[1];
 4725   
 4726     /* Sum: '<S54>/Sum' incorporates:
 4727      *  Sum: '<S71>/Sum'
 4728      */
 4729     Controller_B.Sum_aq[0] = (rtb_TmpSignalConversionAtSFun_g[0] + rtb_Sum_g[0]) +
 4730       (rtb_Gain1_l_idx_1 - rtb_Sum_be[0]);
 4731     Controller_B.Sum_aq[1] = (rtb_TmpSignalConversionAtSFun_g[1] + rtb_Sum_g[1]) +
 4732       (rtb_Gain1_l_idx_0 - rtb_Sum_be[1]);
 4733     Controller_B.Sum_aq[2] = (rtb_TmpSignalConversionAtSFun_g[2] + rtb_Sum_g[2]) +
 4734       (rtb_Gain1_l_idx - rtb_Sum_be[2]);
 4735   
 4736     /* Sum: '<S54>/Sum1' */
 4737     rtb_ypm[0] = rtb_Gain5_g + Controller_B.Output_o;
 4738     rtb_ypm[1] = rtb_TransferFcn1_o + Controller_B.Output_o;
 4739     rtb_ypm[2] = rtb_TransferFcn2_h + Controller_B.Output_o;
 4740     if (rtmIsMajorTimeStep(Controller_M)) {
 4741       /* Gain: '<S75>/Output' incorporates:
 4742        *  RandomNumber: '<S75>/White Noise'
 4743        */
 4744       Controller_B.Output_h[0] = Controller_P.Output_Gain_p[0] *
 4745         Controller_DWork.NextOutput_m[0];
 4746       Controller_B.Output_h[1] = Controller_P.Output_Gain_p[1] *
 4747         Controller_DWork.NextOutput_m[1];
 4748       Controller_B.Output_h[2] = Controller_P.Output_Gain_p[2] *
 4749         Controller_DWork.NextOutput_m[2];
 4750     }
 4751   
 4752     /* TransferFcn: '<S76>/Transfer Fcn' */
 4753     rtb_Gain5_g = Controller_P.TransferFcn_C_a[0]*
 4754       Controller_X.TransferFcn_CSTATE_n[0]
 4755       + Controller_P.TransferFcn_C_a[1]*Controller_X.TransferFcn_CSTATE_n[1];
 4756   
 4757     /* TransferFcn: '<S76>/Transfer Fcn1' */
 4758     rtb_TransferFcn1_o = Controller_P.TransferFcn1_C_l[0]*
 4759       Controller_X.TransferFcn1_CSTATE_p[0]
 4760       + Controller_P.TransferFcn1_C_l[1]*Controller_X.TransferFcn1_CSTATE_p[1];
 4761   
 4762     /* TransferFcn: '<S76>/Transfer Fcn2' */
 4763     rtb_TransferFcn2_h = Controller_P.TransferFcn2_C_j[0]*
 4764       Controller_X.TransferFcn2_CSTATE_ck[0]
 4765       + Controller_P.TransferFcn2_C_j[1]*Controller_X.TransferFcn2_CSTATE_ck[1];
 4766   
 4767     /* Sum: '<S45>/Sum' */
 4768     rtb_Sum_g[0] = rtb_Gain5_g + Controller_B.Output_h[0];
 4769     rtb_Sum_g[1] = rtb_TransferFcn1_o + Controller_B.Output_h[1];
 4770     rtb_Sum_g[2] = rtb_TransferFcn2_h + Controller_B.Output_h[2];
 4771     if (rtmIsMajorTimeStep(Controller_M)) {
 4772     }
 4773   
 4774     /* Gain: '<S13>/Gain1' */
 4775     rtb_Gain1_l_idx = Controller_P.Gain1_Gain_fn * rtb_Sum_g[2];
 4776   
 4777     /* Gain: '<S13>/Gain2' */
 4778     rtb_TmpSignalConversionAtSFun_n[0] = Controller_P.Gain2_Gain_k *
 4779       rtb_Integrator_idx_1;
 4780     rtb_TmpSignalConversionAtSFun_n[1] = Controller_P.Gain2_Gain_k *
 4781       rtb_Integrator_idx_0;
 4782     rtb_TmpSignalConversionAtSFun_n[2] = Controller_P.Gain2_Gain_k *
 4783       rtb_Integrator_idx;
 4784   
 4785     /* Gain: '<S13>/Gain4' */
 4786     rtb_TmpSignalConversionAtSFun_g[0] = Controller_P.Gain4_Gain_j *
 4787       rtb_Integrator_idx_1;
 4788     rtb_TmpSignalConversionAtSFun_g[1] = Controller_P.Gain4_Gain_j *
 4789       rtb_Integrator_idx_0;
 4790     rtb_TmpSignalConversionAtSFun_g[2] = Controller_P.Gain4_Gain_j *
 4791       rtb_Integrator_idx;
 4792   
 4793     /* Gain: '<S13>/Gain5' */
 4794     rtb_k_m_r[2] = Controller_P.Gain5_Gain * rtb_Sum_g[2];
 4795     if (rtmIsMajorTimeStep(Controller_M)) {
 4796     }
 4797   
 4798     /* Trigonometry: '<S99>/sincos' */
 4799     rtb_Sum_be[0] = sin(rtb_frc_ctrl[0]);
 4800     rtb_Product_g[0] = cos(rtb_frc_ctrl[0]);
 4801     rtb_Sum_be[1] = sin(rtb_frc_ctrl[1]);
 4802     rtb_Product_g[1] = cos(rtb_frc_ctrl[1]);
 4803     rtb_Sum_be[2] = sin(rtb_frc_ctrl[2]);
 4804     rtb_Product_g[2] = cos(rtb_frc_ctrl[2]);
 4805   
 4806     /* Fcn: '<S99>/Fcn11' */
 4807     rtb_VectorConcatenate_l[0] = rtb_Product_g[1] * rtb_Product_g[0];
 4808   
 4809     /* Fcn: '<S99>/Fcn21' */
 4810     rtb_VectorConcatenate_l[1] = rtb_Sum_be[2] * rtb_Sum_be[1] * rtb_Product_g[0]
 4811       - rtb_Product_g[2] * rtb_Sum_be[0];
 4812   
 4813     /* Fcn: '<S99>/Fcn31' */
 4814     rtb_VectorConcatenate_l[2] = rtb_Product_g[2] * rtb_Sum_be[1] * rtb_Product_g
 4815       [0] + rtb_Sum_be[2] * rtb_Sum_be[0];
 4816   
 4817     /* Fcn: '<S99>/Fcn12' */
 4818     rtb_VectorConcatenate_l[3] = rtb_Product_g[1] * rtb_Sum_be[0];
 4819   
 4820     /* Fcn: '<S99>/Fcn22' */
 4821     rtb_VectorConcatenate_l[4] = rtb_Sum_be[2] * rtb_Sum_be[1] * rtb_Sum_be[0] +
 4822       rtb_Product_g[2] * rtb_Product_g[0];
 4823   
 4824     /* Fcn: '<S99>/Fcn32' */
 4825     rtb_VectorConcatenate_l[5] = rtb_Product_g[2] * rtb_Sum_be[1] * rtb_Sum_be[0]
 4826       - rtb_Sum_be[2] * rtb_Product_g[0];
 4827   
 4828     /* Fcn: '<S99>/Fcn13' */
 4829     rtb_VectorConcatenate_l[6] = -rtb_Sum_be[1];
 4830   
 4831     /* Fcn: '<S99>/Fcn23' */
 4832     rtb_VectorConcatenate_l[7] = rtb_Sum_be[2] * rtb_Product_g[1];
 4833   
 4834     /* Fcn: '<S99>/Fcn33' */
 4835     rtb_VectorConcatenate_l[8] = rtb_Product_g[2] * rtb_Product_g[1];
 4836   
 4837     /* Sum: '<S82>/Sum1' */
 4838     rtb_Gain5_g = rtb_k_m_r[2] - Controller_B.Sum[2];
 4839   
 4840     /* Gain: '<S82>/Gain2' */
 4841     rtb_Divide = Controller_P.Gain2_Gain_n * rtb_Gain5_g;
 4842   
 4843     /* Gain: '<S82>/Gain5' */
 4844     rtb_Gain5_g *= Controller_P.Gain5_Gain_l;
 4845   
 4846     /* Product: '<S90>/Product' incorporates:
 4847      *  Gain: '<S82>/Gain1'
 4848      */
 4849     for (k = 0; k < 3; k++) {
 4850       rtb_VectorConcatenate_p_0[k] = rtb_VectorConcatenate_l[k + 6] *
 4851         rtb_Sum3_c_idx + (rtb_VectorConcatenate_l[k + 3] * rtb_Sum3_c_idx_0 +
 4852                           rtb_VectorConcatenate_l[k] * rtb_Sum3_c_idx_1);
 4853     }
 4854   
 4855     /* End of Product: '<S90>/Product' */
 4856   
 4857     /* Sum: '<S82>/Sum' incorporates:
 4858      *  Gain: '<S82>/Gain1'
 4859      *  Gain: '<S82>/M'
 4860      */
 4861     for (k = 0; k < 3; k++) {
 4862       tmp_2[k] = (((((Controller_P.Gain1_Gain_c[k + 3] *
 4863                       rtb_VectorConcatenate_p_0[1] + Controller_P.Gain1_Gain_c[k] *
 4864                       rtb_VectorConcatenate_p_0[0]) + Controller_P.Gain1_Gain_c[k
 4865                      + 6] * rtb_VectorConcatenate_p_0[2]) + Controller_B.Gain4_k[k])
 4866                    + rtb_TmpSignalConversionAtSFun_g[k]) + rtb_Divide) -
 4867         rtb_sincos_o2[k];
 4868     }
 4869   
 4870     /* End of Sum: '<S82>/Sum' */
 4871   
 4872     /* Gain: '<S82>/M' */
 4873     for (k = 0; k < 3; k++) {
 4874       Controller_B.M[k] = 0.0;
 4875       Controller_B.M[k] += Controller_P.M_Gain[k] * tmp_2[0];
 4876       Controller_B.M[k] += Controller_P.M_Gain[k + 3] * tmp_2[1];
 4877       Controller_B.M[k] += Controller_P.M_Gain[k + 6] * tmp_2[2];
 4878     }
 4879   
 4880     /* Trigonometry: '<S94>/sincos' */
 4881     rtb_sincos_o2[0] = cos(rtb_frc_ctrl[0]);
 4882     rtb_frc_ctrl[0] = sin(rtb_frc_ctrl[0]);
 4883     rtb_sincos_o2[1] = cos(rtb_frc_ctrl[1]);
 4884     rtb_frc_ctrl[1] = sin(rtb_frc_ctrl[1]);
 4885     rtb_Divide = cos(rtb_frc_ctrl[2]);
 4886     rtb_Normalization_idx_0 = sin(rtb_frc_ctrl[2]);
 4887   
 4888     /* Fcn: '<S94>/Fcn11' */
 4889     rtb_VectorConcatenate_o[0] = rtb_sincos_o2[1] * rtb_sincos_o2[0];
 4890   
 4891     /* Fcn: '<S94>/Fcn21' incorporates:
 4892      *  Trigonometry: '<S94>/sincos'
 4893      */
 4894     rtb_VectorConcatenate_o[1] = rtb_Normalization_idx_0 * rtb_frc_ctrl[1] *
 4895       rtb_sincos_o2[0] - rtb_Divide * rtb_frc_ctrl[0];
 4896   
 4897     /* Fcn: '<S94>/Fcn31' incorporates:
 4898      *  Trigonometry: '<S94>/sincos'
 4899      */
 4900     rtb_VectorConcatenate_o[2] = rtb_Divide * rtb_frc_ctrl[1] * rtb_sincos_o2[0] +
 4901       rtb_Normalization_idx_0 * rtb_frc_ctrl[0];
 4902   
 4903     /* Fcn: '<S94>/Fcn12' */
 4904     rtb_VectorConcatenate_o[3] = rtb_sincos_o2[1] * rtb_frc_ctrl[0];
 4905   
 4906     /* Fcn: '<S94>/Fcn22' incorporates:
 4907      *  Trigonometry: '<S94>/sincos'
 4908      */
 4909     rtb_VectorConcatenate_o[4] = rtb_Normalization_idx_0 * rtb_frc_ctrl[1] *
 4910       rtb_frc_ctrl[0] + rtb_Divide * rtb_sincos_o2[0];
 4911   
 4912     /* Fcn: '<S94>/Fcn32' incorporates:
 4913      *  Trigonometry: '<S94>/sincos'
 4914      */
 4915     rtb_VectorConcatenate_o[5] = rtb_Divide * rtb_frc_ctrl[1] * rtb_frc_ctrl[0] -
 4916       rtb_Normalization_idx_0 * rtb_sincos_o2[0];
 4917   
 4918     /* Fcn: '<S94>/Fcn13' */
 4919     rtb_VectorConcatenate_o[6] = -rtb_frc_ctrl[1];
 4920   
 4921     /* Fcn: '<S94>/Fcn23' */
 4922     rtb_VectorConcatenate_o[7] = rtb_Normalization_idx_0 * rtb_sincos_o2[1];
 4923   
 4924     /* Fcn: '<S94>/Fcn33' incorporates:
 4925      *  Trigonometry: '<S94>/sincos'
 4926      */
 4927     rtb_VectorConcatenate_o[8] = rtb_Divide * rtb_sincos_o2[1];
 4928   
 4929     /* Sum: '<S91>/Sum' incorporates:
 4930      *  Constant: '<S91>/Constant'
 4931      */
 4932     Controller_B.Sum_k[0] = rtb_Gain6[0] + Controller_P.Constant_Value_c;
 4933     Controller_B.Sum_k[1] = rtb_Gain6[1] + Controller_P.Constant_Value_c;
 4934     Controller_B.Sum_k[2] = rtb_Gain6[2] + rtb_Gain5_g;
 4935   
 4936     /* Gain: '<S93>/Gain' */
 4937     rtb_Gain_i[0] = Controller_P.Gain_Gain_bh * Controller_B.rst[0];
 4938     rtb_Gain_i[1] = Controller_P.Gain_Gain_bh * Controller_B.rst[1];
 4939     rtb_Gain_i[2] = Controller_P.Gain_Gain_bh * Controller_B.rst[2];
 4940   
 4941     /* Gain: '<S82>/Gain3' incorporates:
 4942      *  Sum: '<S82>/Sum2'
 4943      */
 4944     for (k = 0; k < 3; k++) {
 4945       tmp_2[k] = Controller_P.Gain3_Gain_b[k + 6] * rtb_Sum3_c_idx +
 4946         (Controller_P.Gain3_Gain_b[k + 3] * rtb_Sum3_c_idx_0 +
 4947          Controller_P.Gain3_Gain_b[k] * rtb_Sum3_c_idx_1);
 4948     }
 4949   
 4950     /* End of Gain: '<S82>/Gain3' */
 4951   
 4952     /* Product: '<S88>/Product' incorporates:
 4953      *  Math: '<S95>/Math Function'
 4954      *  Sum: '<S82>/Sum2'
 4955      */
 4956     for (k = 0; k < 3; k++) {
 4957       rtb_VectorConcatenate_p_0[k] = rtb_VectorConcatenate_o[3 * k + 2] *
 4958         Controller_B.Sum[2] + (rtb_VectorConcatenate_o[3 * k + 1] *
 4959         Controller_B.Sum[1] + rtb_VectorConcatenate_o[3 * k] * Controller_B.Sum[0]);
 4960     }
 4961   
 4962     /* End of Product: '<S88>/Product' */
 4963   
 4964     /* Sum: '<S82>/Sum2' */
 4965     Controller_B.Sum2[0] = tmp_2[0] + rtb_VectorConcatenate_p_0[0];
 4966     Controller_B.Sum2[1] = tmp_2[1] + rtb_VectorConcatenate_p_0[1];
 4967     Controller_B.Sum2[2] = tmp_2[2] + rtb_VectorConcatenate_p_0[2];
 4968   
 4969     /* SignalConversion: '<S103>/TmpSignal ConversionAtGainInport1' */
 4970     rtb_Sum1_pd_0[0] = rtb_Sum1_pd_idx;
 4971     rtb_Sum1_pd_0[1] = rtb_Gain1_f;
 4972     rtb_Sum1_pd_0[2] = rtb_Gain_ia;
 4973     rtb_Sum1_pd_0[3] = rtb_TmpSignalConversionAtSFun_d[0];
 4974     rtb_Sum1_pd_0[4] = rtb_TmpSignalConversionAtSFun_d[1];
 4975     rtb_Sum1_pd_0[5] = rtb_TmpSignalConversionAtSFun_d[2];
 4976     rtb_Sum1_pd_0[6] = rtb_TmpSignalConversionAtSFu_gy[0];
 4977     rtb_Sum1_pd_0[7] = rtb_TmpSignalConversionAtSFu_gy[1];
 4978     rtb_Sum1_pd_0[8] = rtb_TmpSignalConversionAtSFu_gy[2];
 4979     rtb_Sum1_pd_0[9] = rtb_ypm[0];
 4980     rtb_Sum1_pd_0[10] = rtb_ypm[1];
 4981     rtb_Sum1_pd_0[11] = rtb_ypm[2];
 4982   
 4983     /* Gain: '<S103>/Gain' incorporates:
 4984      *  SignalConversion: '<S103>/TmpSignal ConversionAtGainInport1'
 4985      */
 4986     for (k = 0; k < 12; k++) {
 4987       rtb_theta_ctrl_hat[k] = 0.0;
 4988       for (i = 0; i < 12; i++) {
 4989         rtb_theta_ctrl_hat[k] += Controller_P.Gain_Gain_n[12 * i + k] *
 4990           rtb_Sum1_pd_0[i];
 4991       }
 4992     }
 4993   
 4994     /* End of Gain: '<S103>/Gain' */
 4995   
 4996     /* Sum: '<S104>/Sum2' incorporates:
 4997      *  Inport: '<Root>/D'
 4998      *  Inport: '<Root>/E'
 4999      */
 5000     rtb_Sum2_a_idx_1 = rtb_Step - rtb_Sum2_a_idx_1;
 5001     rtb_Sum2_a_idx_0 = Controller_U.E - rtb_Sum2_a_idx_0;
 5002     rtb_Normalization_idx = Controller_U.D - rtb_Sum2_a_idx;
 5003   
 5004     /* Trigonometry: '<S109>/sincos' */
 5005     rtb_TmpSignalConversionAtSFun_d[0] = cos(rtb_sincos_o1_idx_1);
 5006     rtb_sincos_o1_idx_1 = sin(rtb_sincos_o1_idx_1);
 5007     rtb_TmpSignalConversionAtSFun_d[1] = cos(rtb_sincos_o1_idx_0);
 5008     rtb_sincos_o1_idx_0 = sin(rtb_sincos_o1_idx_0);
 5009     rtb_Divide = cos(rtb_sincos_o1_idx);
 5010     rtb_Normalization_idx_0 = sin(rtb_sincos_o1_idx);
 5011   
 5012     /* Fcn: '<S109>/Fcn11' */
 5013     rtb_VectorConcatenate_kr[0] = rtb_TmpSignalConversionAtSFun_d[1] *
 5014       rtb_TmpSignalConversionAtSFun_d[0];
 5015   
 5016     /* Fcn: '<S109>/Fcn21' incorporates:
 5017      *  Trigonometry: '<S109>/sincos'
 5018      */
 5019     rtb_VectorConcatenate_kr[1] = rtb_Normalization_idx_0 * rtb_sincos_o1_idx_0 *
 5020       rtb_TmpSignalConversionAtSFun_d[0] - rtb_Divide * rtb_sincos_o1_idx_1;
 5021   
 5022     /* Fcn: '<S109>/Fcn31' incorporates:
 5023      *  Trigonometry: '<S109>/sincos'
 5024      */
 5025     rtb_VectorConcatenate_kr[2] = rtb_Divide * rtb_sincos_o1_idx_0 *
 5026       rtb_TmpSignalConversionAtSFun_d[0] + rtb_Normalization_idx_0 *
 5027       rtb_sincos_o1_idx_1;
 5028   
 5029     /* Fcn: '<S109>/Fcn12' */
 5030     rtb_VectorConcatenate_kr[3] = rtb_TmpSignalConversionAtSFun_d[1] *
 5031       rtb_sincos_o1_idx_1;
 5032   
 5033     /* Fcn: '<S109>/Fcn22' incorporates:
 5034      *  Trigonometry: '<S109>/sincos'
 5035      */
 5036     rtb_VectorConcatenate_kr[4] = rtb_Normalization_idx_0 * rtb_sincos_o1_idx_0 *
 5037       rtb_sincos_o1_idx_1 + rtb_Divide * rtb_TmpSignalConversionAtSFun_d[0];
 5038   
 5039     /* Fcn: '<S109>/Fcn32' incorporates:
 5040      *  Trigonometry: '<S109>/sincos'
 5041      */
 5042     rtb_VectorConcatenate_kr[5] = rtb_Divide * rtb_sincos_o1_idx_0 *
 5043       rtb_sincos_o1_idx_1 - rtb_Normalization_idx_0 *
 5044       rtb_TmpSignalConversionAtSFun_d[0];
 5045   
 5046     /* Fcn: '<S109>/Fcn13' */
 5047     rtb_VectorConcatenate_kr[6] = -rtb_sincos_o1_idx_0;
 5048   
 5049     /* Fcn: '<S109>/Fcn23' */
 5050     rtb_VectorConcatenate_kr[7] = rtb_Normalization_idx_0 *
 5051       rtb_TmpSignalConversionAtSFun_d[1];
 5052   
 5053     /* Fcn: '<S109>/Fcn33' incorporates:
 5054      *  Trigonometry: '<S109>/sincos'
 5055      */
 5056     rtb_VectorConcatenate_kr[8] = rtb_Divide * rtb_TmpSignalConversionAtSFun_d[1];
 5057   
 5058     /* Sum: '<S104>/Sum1' incorporates:
 5059      *  Integrator: '<S108>/Integrator'
 5060      *  Math: '<S105>/transpose'
 5061      *  Product: '<S104>/Product'
 5062      */
 5063     for (k = 0; k < 3; k++) {
 5064       rtb_ypm[k] = ((rtb_VectorConcatenate_kr[3 * k + 1] * rtb_theta_ctrl_hat[1] +
 5065                      rtb_VectorConcatenate_kr[3 * k] * rtb_theta_ctrl_hat[0]) +
 5066                     rtb_VectorConcatenate_kr[3 * k + 2] * rtb_theta_ctrl_hat[2]) +
 5067         Controller_X.Integrator_CSTATE_l[k];
 5068     }
 5069   
 5070     /* End of Sum: '<S104>/Sum1' */
 5071   
 5072     /* Sum: '<S104>/Sum3' incorporates:
 5073      *  Gain: '<S104>/Gain1'
 5074      */
 5075     for (k = 0; k < 3; k++) {
 5076       Controller_B.Sum3[k] = ((Controller_P.Gain1_Gain_g[k + 3] * rtb_Sum2_a_idx_0
 5077         + Controller_P.Gain1_Gain_g[k] * rtb_Sum2_a_idx_1) +
 5078         Controller_P.Gain1_Gain_g[k + 6] * rtb_Normalization_idx) +
 5079         rtb_Integrator_d[k];
 5080     }
 5081   
 5082     /* End of Sum: '<S104>/Sum3' */
 5083   
 5084     /* Sum: '<S104>/Sum4' incorporates:
 5085      *  Gain: '<S104>/Gain'
 5086      */
 5087     for (k = 0; k < 3; k++) {
 5088       Controller_B.Sum4[k] = ((Controller_P.Gain_Gain_hw[k + 3] * rtb_Sum2_a_idx_0
 5089         + Controller_P.Gain_Gain_hw[k] * rtb_Sum2_a_idx_1) +
 5090         Controller_P.Gain_Gain_hw[k + 6] * rtb_Normalization_idx) + rtb_ypm[k];
 5091     }
 5092   
 5093     /* End of Sum: '<S104>/Sum4' */
 5094   
 5095     /* Gain: '<S108>/Gain' */
 5096     for (k = 0; k < 3; k++) {
 5097       Controller_B.Gain_n[k] = 0.0;
 5098       Controller_B.Gain_n[k] += Controller_P.Gain_Gain_j[k] * rtb_Sum2_a_idx_1;
 5099       Controller_B.Gain_n[k] += Controller_P.Gain_Gain_j[k + 3] * rtb_Sum2_a_idx_0;
 5100       Controller_B.Gain_n[k] += Controller_P.Gain_Gain_j[k + 6] *
 5101         rtb_Normalization_idx;
 5102     }
 5103   
 5104     /* End of Gain: '<S108>/Gain' */
 5105   
 5106     /* Sum: '<S106>/Sum1' incorporates:
 5107      *  Integrator: '<S106>/Integrator1'
 5108      */
 5109     rtb_TmpSignalConversionAtSFu_gy[0] = rtb_TransferFcn -
 5110       Controller_X.Integrator1_CSTATE_b[0];
 5111     rtb_TmpSignalConversionAtSFu_gy[1] = rtb_TransferFcn1 -
 5112       Controller_X.Integrator1_CSTATE_b[1];
 5113     rtb_TmpSignalConversionAtSFu_gy[2] = rtb_TransferFcn2 -
 5114       Controller_X.Integrator1_CSTATE_b[2];
 5115   
 5116     /* MATLAB Function: '<S106>/MATLAB Function1' incorporates:
 5117      *  SignalConversion: '<S112>/TmpSignal ConversionAt SFunction Inport2'
 5118      */
 5119     /* MATLAB Function 'Controll system/Observers/Kinematic observer/Rot observer/MATLAB Function1': '<S112>:1' */
 5120     /* '<S112>:1:4' roll = attitude(1); */
 5121     /* '<S112>:1:5' pitch = attitude(2); */
 5122     /* DD = [1 sin(pitch)*tan(roll) cos(pitch)*tan(roll); 0 cos(pitch) -sin(pitch); 0 sin(pitch)/cos(roll) cos(pitch)/cos(roll)]; */
 5123     /* '<S112>:1:8' DD_inv = [1 0 -sin(roll);0 cos(pitch) cos(roll)*sin(pitch);0 -sin(pitch) cos(roll)*cos(pitch)]; */
 5124     /* '<S112>:1:10' error_w = DD_inv*error; */
 5125     tmp_3[0] = 1.0;
 5126     tmp_3[3] = 0.0;
 5127     tmp_3[6] = -sin(rtb_TransferFcn);
 5128     tmp_3[1] = 0.0;
 5129     tmp_3[4] = cos(rtb_TransferFcn1);
 5130     tmp_3[7] = cos(rtb_TransferFcn) * sin(rtb_TransferFcn1);
 5131     tmp_3[2] = 0.0;
 5132     tmp_3[5] = -sin(rtb_TransferFcn1);
 5133     tmp_3[8] = cos(rtb_TransferFcn) * cos(rtb_TransferFcn1);
 5134     for (k = 0; k < 3; k++) {
 5135       Controller_B.error_w[k] = 0.0;
 5136       Controller_B.error_w[k] += tmp_3[k] * rtb_TmpSignalConversionAtSFu_gy[0];
 5137       Controller_B.error_w[k] += tmp_3[k + 3] * rtb_TmpSignalConversionAtSFu_gy[1];
 5138       Controller_B.error_w[k] += tmp_3[k + 6] * rtb_TmpSignalConversionAtSFu_gy[2];
 5139     }
 5140   
 5141     /* End of MATLAB Function: '<S106>/MATLAB Function1' */
 5142   
 5143     /* Sum: '<S106>/Sum4' incorporates:
 5144      *  Gain: '<S106>/Gain2'
 5145      *  Integrator: '<S106>/Integrator2'
 5146      */
 5147     /* MATLAB Function 'Controll system/Observers/Kinematic observer/Rot observer/MATLAB Function': '<S111>:1' */
 5148     /* '<S111>:1:4' roll = attitude(1); */
 5149     /* '<S111>:1:5' pitch = attitude(2); */
 5150     /* '<S111>:1:7' DD = [1 sin(pitch)*tan(roll) cos(pitch)*tan(roll); 0 cos(pitch) -sin(pitch); 0 sin(pitch)/cos(roll) cos(pitch)/cos(roll)]; */
 5151     /* '<S111>:1:10' theta_dot = DD*gyro; */
 5152     rtb_Product_g[0] = Controller_P.Gain2_Gain_a *
 5153       Controller_X.Integrator2_CSTATE_h[0] + rtb_theta_ctrl_hat[3];
 5154     rtb_Product_g[1] = Controller_P.Gain2_Gain_a *
 5155       Controller_X.Integrator2_CSTATE_h[1] + rtb_theta_ctrl_hat[4];
 5156     rtb_Product_g[2] = Controller_P.Gain2_Gain_a *
 5157       Controller_X.Integrator2_CSTATE_h[2] + rtb_theta_ctrl_hat[5];
 5158   
 5159     /* Sum: '<S106>/Sum' incorporates:
 5160      *  Gain: '<S106>/Gain1'
 5161      *  Gain: '<S106>/Gain3'
 5162      *  Sum: '<S106>/Sum3'
 5163      *  Sum: '<S106>/Sum5'
 5164      */
 5165     Controller_B.Sum_o[0] = (rtb_Sum_g[0] - Controller_B.Integrator[0]) *
 5166       Controller_P.Gain1_Gain_gv + (Controller_P.Gain3_Gain_i *
 5167       Controller_B.error_w[0] + rtb_Product_g[0]);
 5168     Controller_B.Sum_o[1] = (rtb_Sum_g[1] - Controller_B.Integrator[1]) *
 5169       Controller_P.Gain1_Gain_gv + (Controller_P.Gain3_Gain_i *
 5170       Controller_B.error_w[1] + rtb_Product_g[1]);
 5171     Controller_B.Sum_o[2] = (rtb_Sum_g[2] - Controller_B.Integrator[2]) *
 5172       Controller_P.Gain1_Gain_gv + (Controller_P.Gain3_Gain_i *
 5173       Controller_B.error_w[2] + rtb_Product_g[2]);
 5174   
 5175     /* MATLAB Function: '<S106>/MATLAB Function' incorporates:
 5176      *  SignalConversion: '<S111>/TmpSignal ConversionAt SFunction Inport1'
 5177      */
 5178     tmp_4[0] = 1.0;
 5179     tmp_4[3] = sin(rtb_TransferFcn1) * tan(rtb_TransferFcn);
 5180     tmp_4[6] = cos(rtb_TransferFcn1) * tan(rtb_TransferFcn);
 5181     tmp_4[1] = 0.0;
 5182     tmp_4[4] = cos(rtb_TransferFcn1);
 5183     tmp_4[7] = -sin(rtb_TransferFcn1);
 5184     tmp_4[2] = 0.0;
 5185     tmp_4[5] = sin(rtb_TransferFcn1) / cos(rtb_TransferFcn);
 5186     tmp_4[8] = cos(rtb_TransferFcn1) / cos(rtb_TransferFcn);
 5187   
 5188     /* Sum: '<S106>/Sum2' incorporates:
 5189      *  Gain: '<S106>/Gain'
 5190      *  MATLAB Function: '<S106>/MATLAB Function'
 5191      */
 5192     for (k = 0; k < 3; k++) {
 5193       Controller_B.Sum2_g[k] = ((tmp_4[k + 3] * Controller_B.Integrator[1] +
 5194         tmp_4[k] * Controller_B.Integrator[0]) + tmp_4[k + 6] *
 5195         Controller_B.Integrator[2]) + Controller_P.Gain_Gain_e *
 5196         rtb_TmpSignalConversionAtSFu_gy[k];
 5197     }
 5198   
 5199     /* End of Sum: '<S106>/Sum2' */
 5200   
 5201     /* Product: '<S107>/Product' incorporates:
 5202      *  SignalConversion: '<S107>/TmpSignal ConversionAtMInport1'
 5203      */
 5204     for (k = 0; k < 3; k++) {
 5205       rtb_VectorConcatenate_p_0[k] = rtb_VectorConcatenate_d[k + 6] * rtb_ypm[2] +
 5206         (rtb_VectorConcatenate_d[k + 3] * rtb_ypm[1] + rtb_VectorConcatenate_d[k] *
 5207          rtb_ypm[0]);
 5208     }
 5209   
 5210     /* End of Product: '<S107>/Product' */
 5211   
 5212     /* SignalConversion: '<S107>/TmpSignal ConversionAtMInport1' */
 5213     rtb_TmpSignalConversionAtSFu_gy[2] = rtb_Product_g[2];
 5214   
 5215     /* Gain: '<S107>/M' incorporates:
 5216      *  SignalConversion: '<S107>/TmpSignal ConversionAtMInport1'
 5217      */
 5218     for (k = 0; k < 3; k++) {
 5219       rtb_Product_g[k] = Controller_P.M_Gain_a[k + 6] *
 5220         rtb_TmpSignalConversionAtSFu_gy[2] + (Controller_P.M_Gain_a[k + 3] *
 5221         rtb_VectorConcatenate_p_0[1] + Controller_P.M_Gain_a[k] *
 5222         rtb_VectorConcatenate_p_0[0]);
 5223     }
 5224   
 5225     /* End of Gain: '<S107>/M' */
 5226   
 5227     /* Sum: '<S107>/Sum5' incorporates:
 5228      *  Sum: '<S107>/Sum'
 5229      */
 5230     rtb_Sum5_o_idx_1 = (rtb_Product_g[0] - rtb_Integrator_idx_1) -
 5231       rtb_Sum5_o_idx_1;
 5232     rtb_Sum5_o_idx_0 = (rtb_Product_g[1] - rtb_Integrator_idx_0) -
 5233       rtb_Sum5_o_idx_0;
 5234     rtb_Normalization_idx_0 = (rtb_Product_g[2] - rtb_Integrator_idx) -
 5235       rtb_Sum5_o_idx;
 5236   
 5237     /* Gain: '<S107>/Gain2' */
 5238     for (k = 0; k < 3; k++) {
 5239       Controller_B.Gain2[k] = 0.0;
 5240       Controller_B.Gain2[k] += Controller_P.Gain2_Gain_ni[k] * rtb_Sum5_o_idx_1;
 5241       Controller_B.Gain2[k] += Controller_P.Gain2_Gain_ni[k + 3] *
 5242         rtb_Sum5_o_idx_0;
 5243       Controller_B.Gain2[k] += Controller_P.Gain2_Gain_ni[k + 6] *
 5244         rtb_Normalization_idx_0;
 5245     }
 5246   
 5247     /* End of Gain: '<S107>/Gain2' */
 5248     if (rtmIsMajorTimeStep(Controller_M)) {
 5249     }
 5250   
 5251     /* Trigonometry: '<S125>/sincos' incorporates:
 5252      *  SignalConversion: '<S120>/TmpSignal ConversionAtsincosInport1'
 5253      */
 5254     rtb_Product_g[0] = sin(rtb_Integrator6[5]);
 5255     rtb_TmpSignalConversionAtSFu_gy[0] = cos(rtb_Integrator6[5]);
 5256     rtb_Product_g[1] = sin(rtb_Integrator6[4]);
 5257     rtb_TmpSignalConversionAtSFu_gy[1] = cos(rtb_Integrator6[4]);
 5258     rtb_Product_g[2] = sin(rtb_Integrator6[3]);
 5259     rtb_TmpSignalConversionAtSFu_gy[2] = cos(rtb_Integrator6[3]);
 5260   
 5261     /* Fcn: '<S125>/Fcn11' */
 5262     rtb_VectorConcatenate_b[0] = rtb_TmpSignalConversionAtSFu_gy[1] *
 5263       rtb_TmpSignalConversionAtSFu_gy[0];
 5264   
 5265     /* Fcn: '<S125>/Fcn21' */
 5266     rtb_VectorConcatenate_b[1] = rtb_Product_g[2] * rtb_Product_g[1] *
 5267       rtb_TmpSignalConversionAtSFu_gy[0] - rtb_TmpSignalConversionAtSFu_gy[2] *
 5268       rtb_Product_g[0];
 5269   
 5270     /* Fcn: '<S125>/Fcn31' */
 5271     rtb_VectorConcatenate_b[2] = rtb_TmpSignalConversionAtSFu_gy[2] *
 5272       rtb_Product_g[1] * rtb_TmpSignalConversionAtSFu_gy[0] + rtb_Product_g[2] *
 5273       rtb_Product_g[0];
 5274   
 5275     /* Fcn: '<S125>/Fcn12' */
 5276     rtb_VectorConcatenate_b[3] = rtb_TmpSignalConversionAtSFu_gy[1] *
 5277       rtb_Product_g[0];
 5278   
 5279     /* Fcn: '<S125>/Fcn22' */
 5280     rtb_VectorConcatenate_b[4] = rtb_Product_g[2] * rtb_Product_g[1] *
 5281       rtb_Product_g[0] + rtb_TmpSignalConversionAtSFu_gy[2] *
 5282       rtb_TmpSignalConversionAtSFu_gy[0];
 5283   
 5284     /* Fcn: '<S125>/Fcn32' */
 5285     rtb_VectorConcatenate_b[5] = rtb_TmpSignalConversionAtSFu_gy[2] *
 5286       rtb_Product_g[1] * rtb_Product_g[0] - rtb_Product_g[2] *
 5287       rtb_TmpSignalConversionAtSFu_gy[0];
 5288   
 5289     /* Fcn: '<S125>/Fcn13' */
 5290     rtb_VectorConcatenate_b[6] = -rtb_Product_g[1];
 5291   
 5292     /* Fcn: '<S125>/Fcn23' */
 5293     rtb_VectorConcatenate_b[7] = rtb_Product_g[2] *
 5294       rtb_TmpSignalConversionAtSFu_gy[1];
 5295   
 5296     /* Fcn: '<S125>/Fcn33' */
 5297     rtb_VectorConcatenate_b[8] = rtb_TmpSignalConversionAtSFu_gy[2] *
 5298       rtb_TmpSignalConversionAtSFu_gy[1];
 5299   
 5300     /* Sum: '<S84>/Sum3' */
 5301     rtb_Product_g[0] = rtb_Integrator6[0] - Controller_B.Integrator1[0];
 5302     rtb_Product_g[1] = rtb_Integrator6[1] - Controller_B.Integrator1[1];
 5303     rtb_Product_g[2] = rtb_Integrator6[5] - Controller_B.Integrator1[2];
 5304   
 5305     /* Sum: '<S84>/Sum1' */
 5306     rtb_Step = rtb_Gain1_l_idx - Controller_B.Integrator_o[2];
 5307   
 5308     /* Gain: '<S84>/Gain2' */
 5309     rtb_Divide = Controller_P.Gain2_Gain_l1 * rtb_Step;
 5310   
 5311     /* Gain: '<S84>/Gain3' */
 5312     for (k = 0; k < 3; k++) {
 5313       rtb_TmpSignalConversionAtSFu_gy[k] = Controller_P.Gain3_Gain_bo[k + 6] *
 5314         rtb_Product_g[2] + (Controller_P.Gain3_Gain_bo[k + 3] * rtb_Product_g[1] +
 5315                             Controller_P.Gain3_Gain_bo[k] * rtb_Product_g[0]);
 5316     }
 5317   
 5318     /* End of Gain: '<S84>/Gain3' */
 5319   
 5320     /* Gain: '<S84>/Gain5' */
 5321     rtb_Step *= Controller_P.Gain5_Gain_o;
 5322   
 5323     /* Gain: '<S84>/Gain6' */
 5324     for (k = 0; k < 3; k++) {
 5325       rtb_ypm[k] = Controller_P.Gain6_Gain_k[k + 6] * rtb_Product_g[2] +
 5326         (Controller_P.Gain6_Gain_k[k + 3] * rtb_Product_g[1] +
 5327          Controller_P.Gain6_Gain_k[k] * rtb_Product_g[0]);
 5328     }
 5329   
 5330     /* End of Gain: '<S84>/Gain6' */
 5331   
 5332     /* Product: '<S118>/Product' incorporates:
 5333      *  Gain: '<S84>/Gain1'
 5334      */
 5335     for (k = 0; k < 3; k++) {
 5336       rtb_VectorConcatenate_p_0[k] = rtb_VectorConcatenate_b[k + 6] *
 5337         rtb_Product_g[2] + (rtb_VectorConcatenate_b[k + 3] * rtb_Product_g[1] +
 5338                             rtb_VectorConcatenate_b[k] * rtb_Product_g[0]);
 5339     }
 5340   
 5341     /* End of Product: '<S118>/Product' */
 5342   
 5343     /* Sum: '<S84>/Sum' incorporates:
 5344      *  Gain: '<S84>/Gain1'
 5345      *  Gain: '<S84>/M'
 5346      */
 5347     for (k = 0; k < 3; k++) {
 5348       tmp_2[k] = (((((Controller_P.Gain1_Gain_fq[k + 3] *
 5349                       rtb_VectorConcatenate_p_0[1] + Controller_P.Gain1_Gain_fq[k]
 5350                       * rtb_VectorConcatenate_p_0[0]) +
 5351                      Controller_P.Gain1_Gain_fq[k + 6] *
 5352                      rtb_VectorConcatenate_p_0[2]) + Controller_B.Gain4[k]) +
 5353                    rtb_TmpSignalConversionAtSFun_n[k]) + rtb_Divide) - rtb_D[k];
 5354     }
 5355   
 5356     /* End of Sum: '<S84>/Sum' */
 5357   
 5358     /* Gain: '<S84>/M' */
 5359     for (k = 0; k < 3; k++) {
 5360       Controller_B.M_j[k] = 0.0;
 5361       Controller_B.M_j[k] += Controller_P.M_Gain_g[k] * tmp_2[0];
 5362       Controller_B.M_j[k] += Controller_P.M_Gain_g[k + 3] * tmp_2[1];
 5363       Controller_B.M_j[k] += Controller_P.M_Gain_g[k + 6] * tmp_2[2];
 5364     }
 5365   
 5366     /* Trigonometry: '<S120>/sincos' incorporates:
 5367      *  SignalConversion: '<S120>/TmpSignal ConversionAtsincosInport1'
 5368      */
 5369     rtb_Product_g[0] = cos(rtb_Integrator6[5]);
 5370     rtb_Normalization_idx_0 = sin(rtb_Integrator6[5]);
 5371     rtb_Product_g[1] = cos(rtb_sincos_o1_e_idx);
 5372     rtb_sincos_o1_e_idx = sin(rtb_sincos_o1_e_idx);
 5373     rtb_Divide = cos(rtb_Integrator6[3]);
 5374     rtb_Normalization_idx = sin(rtb_Integrator6[3]);
 5375   
 5376     /* Fcn: '<S120>/Fcn11' */
 5377     rtb_VectorConcatenate_ou[0] = rtb_Product_g[1] * rtb_Product_g[0];
 5378   
 5379     /* Fcn: '<S120>/Fcn21' incorporates:
 5380      *  Trigonometry: '<S120>/sincos'
 5381      */
 5382     rtb_VectorConcatenate_ou[1] = rtb_Normalization_idx * rtb_sincos_o1_e_idx *
 5383       rtb_Product_g[0] - rtb_Divide * rtb_Normalization_idx_0;
 5384   
 5385     /* Fcn: '<S120>/Fcn31' incorporates:
 5386      *  Trigonometry: '<S120>/sincos'
 5387      */
 5388     rtb_VectorConcatenate_ou[2] = rtb_Divide * rtb_sincos_o1_e_idx *
 5389       rtb_Product_g[0] + rtb_Normalization_idx * rtb_Normalization_idx_0;
 5390   
 5391     /* Fcn: '<S120>/Fcn12' */
 5392     rtb_VectorConcatenate_ou[3] = rtb_Product_g[1] * rtb_Normalization_idx_0;
 5393   
 5394     /* Fcn: '<S120>/Fcn22' incorporates:
 5395      *  Trigonometry: '<S120>/sincos'
 5396      */
 5397     rtb_VectorConcatenate_ou[4] = rtb_Normalization_idx * rtb_sincos_o1_e_idx *
 5398       rtb_Normalization_idx_0 + rtb_Divide * rtb_Product_g[0];
 5399   
 5400     /* Fcn: '<S120>/Fcn32' incorporates:
 5401      *  Trigonometry: '<S120>/sincos'
 5402      */
 5403     rtb_VectorConcatenate_ou[5] = rtb_Divide * rtb_sincos_o1_e_idx *
 5404       rtb_Normalization_idx_0 - rtb_Normalization_idx * rtb_Product_g[0];
 5405   
 5406     /* Fcn: '<S120>/Fcn13' */
 5407     rtb_VectorConcatenate_ou[6] = -rtb_sincos_o1_e_idx;
 5408   
 5409     /* Fcn: '<S120>/Fcn23' */
 5410     rtb_VectorConcatenate_ou[7] = rtb_Normalization_idx * rtb_Product_g[1];
 5411   
 5412     /* Fcn: '<S120>/Fcn33' incorporates:
 5413      *  Trigonometry: '<S120>/sincos'
 5414      */
 5415     rtb_VectorConcatenate_ou[8] = rtb_Divide * rtb_Product_g[1];
 5416   
 5417     /* Sum: '<S119>/Sum' incorporates:
 5418      *  Constant: '<S119>/Constant'
 5419      */
 5420     rtb_ypm[0] += Controller_P.Constant_Value_k;
 5421     rtb_ypm[1] += Controller_P.Constant_Value_k;
 5422   
 5423     /* Sum: '<S119>/Sum1' incorporates:
 5424      *  Gain: '<S119>/Gain'
 5425      *  Sum: '<S119>/Sum'
 5426      */
 5427     Controller_B.Sum1[0] = rtb_ypm[0] - Controller_P.Gain_Gain_k3 *
 5428       rtb_Integrator2_n_idx_1;
 5429     Controller_B.Sum1[1] = rtb_ypm[1] - Controller_P.Gain_Gain_k3 *
 5430       rtb_Integrator2_n_idx_0;
 5431     Controller_B.Sum1[2] = (rtb_ypm[2] + rtb_Step) - Controller_P.Gain_Gain_k3 *
 5432       rtb_Integrator2_n_idx;
 5433   
 5434     /* Sum: '<S84>/Sum2' incorporates:
 5435      *  Math: '<S121>/Math Function'
 5436      *  Product: '<S116>/Product'
 5437      */
 5438     for (k = 0; k < 3; k++) {
 5439       Controller_B.Sum2_h[k] = ((rtb_VectorConcatenate_ou[3 * k + 1] *
 5440         Controller_B.Integrator_o[1] + rtb_VectorConcatenate_ou[3 * k] *
 5441         Controller_B.Integrator_o[0]) + rtb_VectorConcatenate_ou[3 * k + 2] *
 5442         Controller_B.Integrator_o[2]) + rtb_TmpSignalConversionAtSFu_gy[k];
 5443     }
 5444   
 5445     /* End of Sum: '<S84>/Sum2' */
 5446   
 5447     /* Gain: '<S14>/Gain1' incorporates:
 5448      *  Sum: '<S14>/Sum'
 5449      */
 5450     Controller_B.Gain1_b[0] = (rtb_Gain_pk[0] - rtb_Integrator_idx_1) *
 5451       Controller_P.Gain1_Gain_kt;
 5452     Controller_B.Gain1_b[1] = (rtb_Gain_pk[1] - rtb_Integrator_idx_0) *
 5453       Controller_P.Gain1_Gain_kt;
 5454     Controller_B.Gain1_b[2] = (rtb_Gain_pk[2] - rtb_Integrator_idx) *
 5455       Controller_P.Gain1_Gain_kt;
 5456   
 5457     /* Step: '<S3>/Step' */
 5458     if (Controller_M->Timing.t[0] < Controller_P.Step_Time_p) {
 5459       rtb_Normalization_idx = Controller_P.Step_Y0_m;
 5460     } else {
 5461       rtb_Normalization_idx = Controller_P.Step_YFinal_a;
 5462     }
 5463   
 5464     /* End of Step: '<S3>/Step' */
 5465   
 5466     /* Product: '<S3>/Product' incorporates:
 5467      *  Inport: '<Root>/v_r'
 5468      */
 5469     Controller_B.Product = Controller_U.v_r * rtb_Normalization_idx;
 5470     if (rtmIsMajorTimeStep(Controller_M)) {
 5471       /* Outport: '<Root>/tau_Z' incorporates:
 5472        *  Constant: '<Root>/Constant'
 5473        */
 5474       Controller_Y.tau_Z = Controller_P.Constant_Value_f[0];
 5475   
 5476       /* Outport: '<Root>/tau_K' incorporates:
 5477        *  Constant: '<Root>/Constant'
 5478        */
 5479       Controller_Y.tau_K = Controller_P.Constant_Value_f[1];
 5480   
 5481       /* Outport: '<Root>/tau_M' incorporates:
 5482        *  Constant: '<Root>/Constant'
 5483        */
 5484       Controller_Y.tau_M = Controller_P.Constant_Value_f[2];
 5485     }
 5486   
 5487     if (rtmIsMajorTimeStep(Controller_M)) {
 5488       /* Matfile logging */
 5489       rt_UpdateTXYLogVars(Controller_M->rtwLogInfo, (Controller_M->Timing.t));
 5490     }                                    /* end MajorTimeStep */
 5491   
 5492     if (rtmIsMajorTimeStep(Controller_M)) {
 5493       /* Update for TransportDelay: '<S93>/Transport Delay' */
 5494       {
 5495         real_T **uBuffer = (real_T**)
 5496           &Controller_DWork.TransportDelay_PWORK.TUbufferPtrs[0];
 5497         real_T **tBuffer = (real_T**)
 5498           &Controller_DWork.TransportDelay_PWORK.TUbufferPtrs[3];
 5499         real_T simTime = Controller_M->Timing.t[0];
 5500         boolean_T bufferisfull = FALSE;
 5501         Controller_DWork.TransportDelay_IWORK.Head[0] =
 5502           ((Controller_DWork.TransportDelay_IWORK.Head[0] <
 5503             (Controller_DWork.TransportDelay_IWORK.CircularBufSize[0]-1)) ?
 5504            (Controller_DWork.TransportDelay_IWORK.Head[0]+1) : 0);
 5505         if (Controller_DWork.TransportDelay_IWORK.Head[0] ==
 5506             Controller_DWork.TransportDelay_IWORK.Tail[0]) {
 5507           bufferisfull = TRUE;
 5508           Controller_DWork.TransportDelay_IWORK.Tail[0] =
 5509             ((Controller_DWork.TransportDelay_IWORK.Tail[0] <
 5510               (Controller_DWork.TransportDelay_IWORK.CircularBufSize[0]-1)) ?
 5511              (Controller_DWork.TransportDelay_IWORK.Tail[0]+1) : 0);
 5512         }
 5513   
 5514         (*tBuffer++)[Controller_DWork.TransportDelay_IWORK.Head[0]] = simTime;
 5515         (*uBuffer++)[Controller_DWork.TransportDelay_IWORK.Head[0]] = rtb_Gain_i[0];
 5516         Controller_DWork.TransportDelay_IWORK.Head[1] =
 5517           ((Controller_DWork.TransportDelay_IWORK.Head[1] <
 5518             (Controller_DWork.TransportDelay_IWORK.CircularBufSize[1]-1)) ?
 5519            (Controller_DWork.TransportDelay_IWORK.Head[1]+1) : 0);
 5520         if (Controller_DWork.TransportDelay_IWORK.Head[1] ==
 5521             Controller_DWork.TransportDelay_IWORK.Tail[1]) {
 5522           bufferisfull = TRUE;
 5523           Controller_DWork.TransportDelay_IWORK.Tail[1] =
 5524             ((Controller_DWork.TransportDelay_IWORK.Tail[1] <
 5525               (Controller_DWork.TransportDelay_IWORK.CircularBufSize[1]-1)) ?
 5526              (Controller_DWork.TransportDelay_IWORK.Tail[1]+1) : 0);
 5527         }
 5528   
 5529         (*tBuffer++)[Controller_DWork.TransportDelay_IWORK.Head[1]] = simTime;
 5530         (*uBuffer++)[Controller_DWork.TransportDelay_IWORK.Head[1]] = rtb_Gain_i[1];
 5531         Controller_DWork.TransportDelay_IWORK.Head[2] =
 5532           ((Controller_DWork.TransportDelay_IWORK.Head[2] <
 5533             (Controller_DWork.TransportDelay_IWORK.CircularBufSize[2]-1)) ?
 5534            (Controller_DWork.TransportDelay_IWORK.Head[2]+1) : 0);
 5535         if (Controller_DWork.TransportDelay_IWORK.Head[2] ==
 5536             Controller_DWork.TransportDelay_IWORK.Tail[2]) {
 5537           bufferisfull = TRUE;
 5538           Controller_DWork.TransportDelay_IWORK.Tail[2] =
 5539             ((Controller_DWork.TransportDelay_IWORK.Tail[2] <
 5540               (Controller_DWork.TransportDelay_IWORK.CircularBufSize[2]-1)) ?
 5541              (Controller_DWork.TransportDelay_IWORK.Tail[2]+1) : 0);
 5542         }
 5543   
 5544         (*tBuffer)[Controller_DWork.TransportDelay_IWORK.Head[2]] = simTime;
 5545         (*uBuffer)[Controller_DWork.TransportDelay_IWORK.Head[2]] = rtb_Gain_i[2];
 5546         if (bufferisfull) {
 5547           rtsiSetSolverNeedsReset(&Controller_M->solverInfo, TRUE);
 5548           rtsiSetBlkStateChange(&Controller_M->solverInfo, TRUE);
 5549         }
 5550       }
 5551   
 5552       /* Update for Integrator: '<S93>/Integrator1' */
 5553       Controller_DWork.Integrator1_IWORK.IcNeedsLoading = 0;
 5554   
 5555       /* Update for Integrator: '<S93>/Integrator2' */
 5556       Controller_DWork.Integrator2_IWORK.IcNeedsLoading = 0;
 5557   
 5558       /* Update for Integrator: '<S93>/Integrator3' */
 5559       Controller_DWork.Integrator3_IWORK.IcNeedsLoading = 0;
 5560       if (rtmIsMajorTimeStep(Controller_M)) {
 5561         /* Update for RandomNumber: '<S58>/White Noise' */
 5562         Controller_DWork.NextOutput = rt_nrand_Upu32_Yd_f_pw_snf
 5563           (&Controller_DWork.RandSeed) * Controller_P.WhiteNoise_StdDev +
 5564           Controller_P.WhiteNoise_Mean;
 5565       }
 5566   
 5567       if (rtmIsMajorTimeStep(Controller_M)) {
 5568         /* Update for RandomNumber: '<S63>/White Noise' */
 5569         Controller_DWork.NextOutput_k = rt_nrand_Upu32_Yd_f_pw_snf
 5570           (&Controller_DWork.RandSeed_f) * Controller_P.WhiteNoise_StdDev_b +
 5571           Controller_P.WhiteNoise_Mean_f;
 5572       }
 5573   
 5574       if (rtmIsMajorTimeStep(Controller_M)) {
 5575         /* Update for RandomNumber: '<S68>/White Noise' */
 5576         Controller_DWork.NextOutput_f = rt_nrand_Upu32_Yd_f_pw_snf
 5577           (&Controller_DWork.RandSeed_p) * Controller_P.WhiteNoise_StdDev_h +
 5578           Controller_P.WhiteNoise_Mean_g;
 5579       }
 5580   
 5581       if (rtmIsMajorTimeStep(Controller_M)) {
 5582         /* Update for RandomNumber: '<S73>/White Noise' */
 5583         Controller_DWork.NextOutput_c = rt_nrand_Upu32_Yd_f_pw_snf
 5584           (&Controller_DWork.RandSeed_a) * Controller_P.WhiteNoise_StdDev_d +
 5585           Controller_P.WhiteNoise_Mean_j;
 5586       }
 5587   
 5588       if (rtmIsMajorTimeStep(Controller_M)) {
 5589         /* Update for RandomNumber: '<S75>/White Noise' */
 5590         Controller_DWork.NextOutput_m[0] = rt_nrand_Upu32_Yd_f_pw_snf
 5591           (&Controller_DWork.RandSeed_fl[0]) * Controller_P.WhiteNoise_StdDev_j +
 5592           Controller_P.WhiteNoise_Mean_p;
 5593         Controller_DWork.NextOutput_m[1] = rt_nrand_Upu32_Yd_f_pw_snf
 5594           (&Controller_DWork.RandSeed_fl[1]) * Controller_P.WhiteNoise_StdDev_j +
 5595           Controller_P.WhiteNoise_Mean_p;
 5596         Controller_DWork.NextOutput_m[2] = rt_nrand_Upu32_Yd_f_pw_snf
 5597           (&Controller_DWork.RandSeed_fl[2]) * Controller_P.WhiteNoise_StdDev_j +
 5598           Controller_P.WhiteNoise_Mean_p;
 5599       }
 5600   
 5601       /* BlkStateChangeFlag is set, need to run a minor output */
 5602       if (rtmIsMajorTimeStep(Controller_M)) {
 5603         if (rtsiGetBlkStateChange(&Controller_M->solverInfo)) {
 5604           rtsiSetSimTimeStep(&Controller_M->solverInfo,MINOR_TIME_STEP);
 5605           rtsiSetBlkStateChange(&Controller_M->solverInfo, FALSE);
 5606           Controller_step();
 5607           rtsiSetSimTimeStep(&Controller_M->solverInfo, MAJOR_TIME_STEP);
 5608         }
 5609       }
 5610     }                                    /* end MajorTimeStep */
 5611   
 5612     if (rtmIsMajorTimeStep(Controller_M)) {
 5613       /* signal main to stop simulation */
 5614       {                                  /* Sample time: [0.0s, 0.0s] */
 5615         if ((rtmGetTFinal(Controller_M)!=-1) &&
 5616             !((rtmGetTFinal(Controller_M)-(((Controller_M->Timing.clockTick1+
 5617                  Controller_M->Timing.clockTickH1* 4294967296.0)) * 0.01)) >
 5618               (((Controller_M->Timing.clockTick1+Controller_M->Timing.clockTickH1*
 5619                  4294967296.0)) * 0.01) * (DBL_EPSILON))) {
 5620           rtmSetErrorStatus(Controller_M, "Simulation finished");
 5621         }
 5622       }
 5623   
 5624       rt_ertODEUpdateContinuousStates(&Controller_M->solverInfo);
 5625   
 5626       /* Update absolute time for base rate */
 5627       /* The "clockTick0" counts the number of times the code of this task has
 5628        * been executed. The absolute time is the multiplication of "clockTick0"
 5629        * and "Timing.stepSize0". Size of "clockTick0" ensures timer will not
 5630        * overflow during the application lifespan selected.
 5631        * Timer of this task consists of two 32 bit unsigned integers.
 5632        * The two integers represent the low bits Timing.clockTick0 and the high bits
 5633        * Timing.clockTickH0. When the low bit overflows to 0, the high bits increment.
 5634        */
 5635       if (!(++Controller_M->Timing.clockTick0)) {
 5636         ++Controller_M->Timing.clockTickH0;
 5637       }
 5638   
 5639       Controller_M->Timing.t[0] = rtsiGetSolverStopTime(&Controller_M->solverInfo);
 5640   
 5641       {
 5642         /* Update absolute timer for sample time: [0.01s, 0.0s] */
 5643         /* The "clockTick1" counts the number of times the code of this task has
 5644          * been executed. The resolution of this integer timer is 0.01, which is the step size
 5645          * of the task. Size of "clockTick1" ensures timer will not overflow during the
 5646          * application lifespan selected.
 5647          * Timer of this task consists of two 32 bit unsigned integers.
 5648          * The two integers represent the low bits Timing.clockTick1 and the high bits
 5649          * Timing.clockTickH1. When the low bit overflows to 0, the high bits increment.
 5650          */
 5651         Controller_M->Timing.clockTick1++;
 5652         if (!Controller_M->Timing.clockTick1) {
 5653           Controller_M->Timing.clockTickH1++;
 5654         }
 5655       }
 5656     }                                    /* end MajorTimeStep */
 5657   }
 5658   
 5659   /* Derivatives for root system: '<Root>' */
 5660   void Controller_derivatives(void)
 5661   {
 5662     int32_T i;
 5663     StateDerivatives_Controller *_rtXdot;
 5664     _rtXdot = ((StateDerivatives_Controller *) Controller_M->ModelData.derivs);
 5665   
 5666     /* Derivatives for Integrator: '<S14>/Integrator' */
 5667     _rtXdot->Integrator_CSTATE[0] = Controller_B.Gain1_b[0];
 5668     _rtXdot->Integrator_CSTATE[1] = Controller_B.Gain1_b[1];
 5669     _rtXdot->Integrator_CSTATE[2] = Controller_B.Gain1_b[2];
 5670   
 5671     /* Derivatives for Integrator: '<S104>/Integrator1' */
 5672     _rtXdot->Integrator1_CSTATE[0] = Controller_B.Sum3[0];
 5673     _rtXdot->Integrator1_CSTATE[1] = Controller_B.Sum3[1];
 5674     _rtXdot->Integrator1_CSTATE[2] = Controller_B.Sum3[2];
 5675   
 5676     /* Derivatives for TransferFcn: '<S77>/Transfer Fcn2' */
 5677     {
 5678       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5679         ->TransferFcn2_CSTATE[0] = Controller_U.yaw;
 5680       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5681         ->TransferFcn2_CSTATE[0] += (Controller_P.TransferFcn2_A[0])*
 5682         Controller_X.TransferFcn2_CSTATE[0]
 5683         + (Controller_P.TransferFcn2_A[1])*Controller_X.TransferFcn2_CSTATE[1];
 5684       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5685         ->TransferFcn2_CSTATE[1]= Controller_X.TransferFcn2_CSTATE[0];
 5686     }
 5687   
 5688     /* Derivatives for TransferFcn: '<S77>/Transfer Fcn1' */
 5689     {
 5690       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5691         ->TransferFcn1_CSTATE[0] = Controller_U.pitch;
 5692       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5693         ->TransferFcn1_CSTATE[0] += (Controller_P.TransferFcn1_A[0])*
 5694         Controller_X.TransferFcn1_CSTATE[0]
 5695         + (Controller_P.TransferFcn1_A[1])*Controller_X.TransferFcn1_CSTATE[1];
 5696       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5697         ->TransferFcn1_CSTATE[1]= Controller_X.TransferFcn1_CSTATE[0];
 5698     }
 5699   
 5700     /* Derivatives for TransferFcn: '<S77>/Transfer Fcn' */
 5701     {
 5702       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5703         ->TransferFcn_CSTATE[0] = Controller_U.roll;
 5704       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5705         ->TransferFcn_CSTATE[0] += (Controller_P.TransferFcn_A[0])*
 5706         Controller_X.TransferFcn_CSTATE[0]
 5707         + (Controller_P.TransferFcn_A[1])*Controller_X.TransferFcn_CSTATE[1];
 5708       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5709         ->TransferFcn_CSTATE[1]= Controller_X.TransferFcn_CSTATE[0];
 5710     }
 5711   
 5712     /* Derivatives for Integrator: '<S104>/Integrator' */
 5713     _rtXdot->Integrator_CSTATE_g[0] = Controller_B.Sum4[0];
 5714     _rtXdot->Integrator_CSTATE_g[1] = Controller_B.Sum4[1];
 5715     _rtXdot->Integrator_CSTATE_g[2] = Controller_B.Sum4[2];
 5716   
 5717     /* Derivatives for Integrator: '<S106>/Integrator' */
 5718     _rtXdot->Integrator_CSTATE_f[0] = Controller_B.Sum_o[0];
 5719     _rtXdot->Integrator_CSTATE_f[1] = Controller_B.Sum_o[1];
 5720     _rtXdot->Integrator_CSTATE_f[2] = Controller_B.Sum_o[2];
 5721   
 5722     /* Derivatives for Integrator: '<S107>/Integrator2' */
 5723     _rtXdot->Integrator2_CSTATE[0] = Controller_B.Gain2[0];
 5724     _rtXdot->Integrator2_CSTATE[1] = Controller_B.Gain2[1];
 5725     _rtXdot->Integrator2_CSTATE[2] = Controller_B.Gain2[2];
 5726     for (i = 0; i < 6; i++) {
 5727       /* Derivatives for Integrator: '<S16>/Integrator2' */
 5728       _rtXdot->Integrator2_CSTATE_f[i] = Controller_B.xm_dot[i];
 5729   
 5730       /* Derivatives for Integrator: '<S16>/Integrator6' */
 5731       _rtXdot->Integrator6_CSTATE[i] = Controller_B.Fm_dot[i];
 5732     }
 5733   
 5734     /* Derivatives for TransferFcn: '<S22>/Transfer Fcn' */
 5735     {
 5736       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5737         ->TransferFcn_CSTATE_e = Controller_B.Row1;
 5738       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5739         ->TransferFcn_CSTATE_e += (Controller_P.TransferFcn_A_p)*
 5740         Controller_X.TransferFcn_CSTATE_e;
 5741     }
 5742   
 5743     /* Derivatives for TransferFcn: '<S22>/Transfer Fcn1' */
 5744     {
 5745       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5746         ->TransferFcn1_CSTATE_d = Controller_B.Row2;
 5747       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5748         ->TransferFcn1_CSTATE_d += (Controller_P.TransferFcn1_A_e)*
 5749         Controller_X.TransferFcn1_CSTATE_d;
 5750     }
 5751   
 5752     /* Derivatives for TransferFcn: '<S22>/Transfer Fcn2' */
 5753     {
 5754       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5755         ->TransferFcn2_CSTATE_b = Controller_B.Row3;
 5756       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5757         ->TransferFcn2_CSTATE_b += (Controller_P.TransferFcn2_A_p)*
 5758         Controller_X.TransferFcn2_CSTATE_b;
 5759     }
 5760   
 5761     for (i = 0; i < 18; i++) {
 5762       /* Derivatives for Integrator: '<S16>/Integrator10' */
 5763       _rtXdot->Integrator10_CSTATE[i] = Controller_B.theta_hat_dot[i];
 5764   
 5765       /* Derivatives for Integrator: '<S16>/Integrator3' */
 5766       _rtXdot->Integrator3_CSTATE[i] = Controller_B.theta_ves_hat_dot[i];
 5767     }
 5768   
 5769     /* Derivatives for Integrator: '<S16>/Integrator1' */
 5770     for (i = 0; i < 9; i++) {
 5771       _rtXdot->Integrator1_CSTATE_m[i] = Controller_B.omega0_dot[i];
 5772     }
 5773   
 5774     /* End of Derivatives for Integrator: '<S16>/Integrator1' */
 5775   
 5776     /* Derivatives for Integrator: '<S15>/Integrator' */
 5777     _rtXdot->Integrator_CSTATE_p[0] =
 5778       Controller_B.TmpSignalConversionAtProduct1In[0];
 5779     _rtXdot->Integrator_CSTATE_p[1] =
 5780       Controller_B.TmpSignalConversionAtProduct1In[1];
 5781     _rtXdot->Integrator_CSTATE_p[2] =
 5782       Controller_B.TmpSignalConversionAtProduct1In[2];
 5783   
 5784     /* Derivatives for Integrator: '<S33>/Integrator1' */
 5785     _rtXdot->Integrator1_CSTATE_f = Controller_B.Integrator_n;
 5786   
 5787     /* Derivatives for Integrator: '<S33>/Integrator' */
 5788     _rtXdot->Integrator_CSTATE_c = Controller_B.Gain1;
 5789   
 5790     /* Derivatives for Integrator: '<S3>/Integrator' */
 5791     _rtXdot->Integrator_CSTATE_j = Controller_B.Product;
 5792   
 5793     /* Derivatives for Integrator: '<S84>/Integrator1' */
 5794     _rtXdot->Integrator1_CSTATE_k[0] = Controller_B.Sum2_h[0];
 5795     _rtXdot->Integrator1_CSTATE_k[1] = Controller_B.Sum2_h[1];
 5796     _rtXdot->Integrator1_CSTATE_k[2] = Controller_B.Sum2_h[2];
 5797   
 5798     /* Derivatives for Integrator: '<S84>/Integrator' */
 5799     _rtXdot->Integrator_CSTATE_px[0] = Controller_B.M_j[0];
 5800     _rtXdot->Integrator_CSTATE_px[1] = Controller_B.M_j[1];
 5801     _rtXdot->Integrator_CSTATE_px[2] = Controller_B.M_j[2];
 5802   
 5803     /* Derivatives for Integrator: '<S119>/Integrator2' */
 5804     _rtXdot->Integrator2_CSTATE_i[0] = Controller_B.Sum1[0];
 5805     _rtXdot->Integrator2_CSTATE_i[1] = Controller_B.Sum1[1];
 5806     _rtXdot->Integrator2_CSTATE_i[2] = Controller_B.Sum1[2];
 5807   
 5808     /* Derivatives for Integrator: '<S93>/Integrator1' */
 5809     {
 5810       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5811         ->Integrator1_CSTATE_g = Controller_B.Sum2[0];
 5812     }
 5813   
 5814     /* Derivatives for Integrator: '<S93>/Integrator2' */
 5815     {
 5816       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5817         ->Integrator2_CSTATE_n = Controller_B.Sum2[1];
 5818     }
 5819   
 5820     /* Derivatives for Integrator: '<S93>/Integrator3' */
 5821     {
 5822       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5823         ->Integrator3_CSTATE_m = Controller_B.Sum2[2];
 5824     }
 5825   
 5826     /* Derivatives for Integrator: '<S92>/Integrator' */
 5827     _rtXdot->Integrator_CSTATE_d[0] = Controller_B.M[0];
 5828     _rtXdot->Integrator_CSTATE_d[1] = Controller_B.M[1];
 5829     _rtXdot->Integrator_CSTATE_d[2] = Controller_B.M[2];
 5830   
 5831     /* Derivatives for Integrator: '<S91>/Integrator2' */
 5832     _rtXdot->Integrator2_CSTATE_n4[0] = Controller_B.Sum_k[0];
 5833     _rtXdot->Integrator2_CSTATE_n4[1] = Controller_B.Sum_k[1];
 5834     _rtXdot->Integrator2_CSTATE_n4[2] = Controller_B.Sum_k[2];
 5835   
 5836     /* Derivatives for Integrator: '<S16>/Integrator11' */
 5837     _rtXdot->Integrator11_CSTATE[0] = Controller_B.m_s_dot_l[0];
 5838     _rtXdot->Integrator11_CSTATE[1] = Controller_B.m_s_dot_l[1];
 5839     _rtXdot->Integrator11_CSTATE[2] = Controller_B.m_s_dot_l[2];
 5840   
 5841     /* Derivatives for Integrator: '<S16>/Integrator4' */
 5842     for (i = 0; i < 30; i++) {
 5843       _rtXdot->Integrator4_CSTATE[i] = Controller_B.phi_dot[i];
 5844     }
 5845   
 5846     /* End of Derivatives for Integrator: '<S16>/Integrator4' */
 5847   
 5848     /* Derivatives for Integrator: '<S16>/Integrator5' */
 5849     _rtXdot->Integrator5_CSTATE[0] = Controller_B.m_s_dot[0];
 5850     _rtXdot->Integrator5_CSTATE[1] = Controller_B.m_s_dot[1];
 5851     _rtXdot->Integrator5_CSTATE[2] = Controller_B.m_s_dot[2];
 5852   
 5853     /* Derivatives for Integrator: '<S16>/Integrator9' */
 5854     for (i = 0; i < 18; i++) {
 5855       _rtXdot->Integrator9_CSTATE[i] = Controller_B.phi_dot_e[i];
 5856     }
 5857   
 5858     /* End of Derivatives for Integrator: '<S16>/Integrator9' */
 5859     /* Derivatives for TransferFcn: '<S59>/Transfer Fcn' */
 5860     {
 5861       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5862         ->TransferFcn_CSTATE_en[0] = Controller_B.Sum_a[0];
 5863       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5864         ->TransferFcn_CSTATE_en[0] += (Controller_P.TransferFcn_A_o[0])*
 5865         Controller_X.TransferFcn_CSTATE_en[0]
 5866         + (Controller_P.TransferFcn_A_o[1])*Controller_X.TransferFcn_CSTATE_en[1];
 5867       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5868         ->TransferFcn_CSTATE_en[1]= Controller_X.TransferFcn_CSTATE_en[0];
 5869     }
 5870   
 5871     /* Derivatives for TransferFcn: '<S59>/Transfer Fcn1' */
 5872     {
 5873       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5874         ->TransferFcn1_CSTATE_m[0] = Controller_B.Sum_a[1];
 5875       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5876         ->TransferFcn1_CSTATE_m[0] += (Controller_P.TransferFcn1_A_g[0])*
 5877         Controller_X.TransferFcn1_CSTATE_m[0]
 5878         + (Controller_P.TransferFcn1_A_g[1])*Controller_X.TransferFcn1_CSTATE_m[1];
 5879       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5880         ->TransferFcn1_CSTATE_m[1]= Controller_X.TransferFcn1_CSTATE_m[0];
 5881     }
 5882   
 5883     /* Derivatives for TransferFcn: '<S59>/Transfer Fcn2' */
 5884     {
 5885       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5886         ->TransferFcn2_CSTATE_j[0] = Controller_B.Sum_a[2];
 5887       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5888         ->TransferFcn2_CSTATE_j[0] += (Controller_P.TransferFcn2_A_g[0])*
 5889         Controller_X.TransferFcn2_CSTATE_j[0]
 5890         + (Controller_P.TransferFcn2_A_g[1])*Controller_X.TransferFcn2_CSTATE_j[1];
 5891       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5892         ->TransferFcn2_CSTATE_j[1]= Controller_X.TransferFcn2_CSTATE_j[0];
 5893     }
 5894   
 5895     /* Derivatives for TransferFcn: '<S64>/Transfer Fcn' */
 5896     {
 5897       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5898         ->TransferFcn_CSTATE_d[0] = Controller_B.Sum_f[0];
 5899       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5900         ->TransferFcn_CSTATE_d[0] += (Controller_P.TransferFcn_A_j[0])*
 5901         Controller_X.TransferFcn_CSTATE_d[0]
 5902         + (Controller_P.TransferFcn_A_j[1])*Controller_X.TransferFcn_CSTATE_d[1];
 5903       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5904         ->TransferFcn_CSTATE_d[1]= Controller_X.TransferFcn_CSTATE_d[0];
 5905     }
 5906   
 5907     /* Derivatives for TransferFcn: '<S64>/Transfer Fcn1' */
 5908     {
 5909       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5910         ->TransferFcn1_CSTATE_e[0] = Controller_B.Sum_f[1];
 5911       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5912         ->TransferFcn1_CSTATE_e[0] += (Controller_P.TransferFcn1_A_b[0])*
 5913         Controller_X.TransferFcn1_CSTATE_e[0]
 5914         + (Controller_P.TransferFcn1_A_b[1])*Controller_X.TransferFcn1_CSTATE_e[1];
 5915       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5916         ->TransferFcn1_CSTATE_e[1]= Controller_X.TransferFcn1_CSTATE_e[0];
 5917     }
 5918   
 5919     /* Derivatives for TransferFcn: '<S64>/Transfer Fcn2' */
 5920     {
 5921       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5922         ->TransferFcn2_CSTATE_c[0] = Controller_B.Sum_f[2];
 5923       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5924         ->TransferFcn2_CSTATE_c[0] += (Controller_P.TransferFcn2_A_f[0])*
 5925         Controller_X.TransferFcn2_CSTATE_c[0]
 5926         + (Controller_P.TransferFcn2_A_f[1])*Controller_X.TransferFcn2_CSTATE_c[1];
 5927       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5928         ->TransferFcn2_CSTATE_c[1]= Controller_X.TransferFcn2_CSTATE_c[0];
 5929     }
 5930   
 5931     /* Derivatives for TransferFcn: '<S69>/Transfer Fcn' */
 5932     {
 5933       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5934         ->TransferFcn_CSTATE_o[0] = Controller_B.Sum_l[0];
 5935       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5936         ->TransferFcn_CSTATE_o[0] += (Controller_P.TransferFcn_A_e[0])*
 5937         Controller_X.TransferFcn_CSTATE_o[0]
 5938         + (Controller_P.TransferFcn_A_e[1])*Controller_X.TransferFcn_CSTATE_o[1];
 5939       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5940         ->TransferFcn_CSTATE_o[1]= Controller_X.TransferFcn_CSTATE_o[0];
 5941     }
 5942   
 5943     /* Derivatives for TransferFcn: '<S69>/Transfer Fcn1' */
 5944     {
 5945       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5946         ->TransferFcn1_CSTATE_g[0] = Controller_B.Sum_l[1];
 5947       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5948         ->TransferFcn1_CSTATE_g[0] += (Controller_P.TransferFcn1_A_f[0])*
 5949         Controller_X.TransferFcn1_CSTATE_g[0]
 5950         + (Controller_P.TransferFcn1_A_f[1])*Controller_X.TransferFcn1_CSTATE_g[1];
 5951       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5952         ->TransferFcn1_CSTATE_g[1]= Controller_X.TransferFcn1_CSTATE_g[0];
 5953     }
 5954   
 5955     /* Derivatives for TransferFcn: '<S69>/Transfer Fcn2' */
 5956     {
 5957       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5958         ->TransferFcn2_CSTATE_i[0] = Controller_B.Sum_l[2];
 5959       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5960         ->TransferFcn2_CSTATE_i[0] += (Controller_P.TransferFcn2_A_h[0])*
 5961         Controller_X.TransferFcn2_CSTATE_i[0]
 5962         + (Controller_P.TransferFcn2_A_h[1])*Controller_X.TransferFcn2_CSTATE_i[1];
 5963       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5964         ->TransferFcn2_CSTATE_i[1]= Controller_X.TransferFcn2_CSTATE_i[0];
 5965     }
 5966   
 5967     /* Derivatives for TransferFcn: '<S74>/Transfer Fcn' */
 5968     {
 5969       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5970         ->TransferFcn_CSTATE_c[0] = Controller_B.Sum_aq[0];
 5971       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5972         ->TransferFcn_CSTATE_c[0] += (Controller_P.TransferFcn_A_d[0])*
 5973         Controller_X.TransferFcn_CSTATE_c[0]
 5974         + (Controller_P.TransferFcn_A_d[1])*Controller_X.TransferFcn_CSTATE_c[1];
 5975       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5976         ->TransferFcn_CSTATE_c[1]= Controller_X.TransferFcn_CSTATE_c[0];
 5977     }
 5978   
 5979     /* Derivatives for TransferFcn: '<S74>/Transfer Fcn1' */
 5980     {
 5981       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5982         ->TransferFcn1_CSTATE_l[0] = Controller_B.Sum_aq[1];
 5983       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5984         ->TransferFcn1_CSTATE_l[0] += (Controller_P.TransferFcn1_A_g2[0])*
 5985         Controller_X.TransferFcn1_CSTATE_l[0]
 5986         + (Controller_P.TransferFcn1_A_g2[1])*Controller_X.TransferFcn1_CSTATE_l[1];
 5987       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5988         ->TransferFcn1_CSTATE_l[1]= Controller_X.TransferFcn1_CSTATE_l[0];
 5989     }
 5990   
 5991     /* Derivatives for TransferFcn: '<S74>/Transfer Fcn2' */
 5992     {
 5993       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5994         ->TransferFcn2_CSTATE_h[0] = Controller_B.Sum_aq[2];
 5995       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 5996         ->TransferFcn2_CSTATE_h[0] += (Controller_P.TransferFcn2_A_k[0])*
 5997         Controller_X.TransferFcn2_CSTATE_h[0]
 5998         + (Controller_P.TransferFcn2_A_k[1])*Controller_X.TransferFcn2_CSTATE_h[1];
 5999       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 6000         ->TransferFcn2_CSTATE_h[1]= Controller_X.TransferFcn2_CSTATE_h[0];
 6001     }
 6002   
 6003     /* Derivatives for TransferFcn: '<S76>/Transfer Fcn' */
 6004     {
 6005       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 6006         ->TransferFcn_CSTATE_n[0] = Controller_B.Product2[0];
 6007       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 6008         ->TransferFcn_CSTATE_n[0] += (Controller_P.TransferFcn_A_p1[0])*
 6009         Controller_X.TransferFcn_CSTATE_n[0]
 6010         + (Controller_P.TransferFcn_A_p1[1])*Controller_X.TransferFcn_CSTATE_n[1];
 6011       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 6012         ->TransferFcn_CSTATE_n[1]= Controller_X.TransferFcn_CSTATE_n[0];
 6013     }
 6014   
 6015     /* Derivatives for TransferFcn: '<S76>/Transfer Fcn1' */
 6016     {
 6017       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 6018         ->TransferFcn1_CSTATE_p[0] = Controller_B.Product2[1];
 6019       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 6020         ->TransferFcn1_CSTATE_p[0] += (Controller_P.TransferFcn1_A_n[0])*
 6021         Controller_X.TransferFcn1_CSTATE_p[0]
 6022         + (Controller_P.TransferFcn1_A_n[1])*Controller_X.TransferFcn1_CSTATE_p[1];
 6023       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 6024         ->TransferFcn1_CSTATE_p[1]= Controller_X.TransferFcn1_CSTATE_p[0];
 6025     }
 6026   
 6027     /* Derivatives for TransferFcn: '<S76>/Transfer Fcn2' */
 6028     {
 6029       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 6030         ->TransferFcn2_CSTATE_ck[0] = Controller_B.Product2[2];
 6031       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 6032         ->TransferFcn2_CSTATE_ck[0] += (Controller_P.TransferFcn2_A_gl[0])*
 6033         Controller_X.TransferFcn2_CSTATE_ck[0]
 6034         + (Controller_P.TransferFcn2_A_gl[1])*Controller_X.TransferFcn2_CSTATE_ck
 6035         [1];
 6036       ((StateDerivatives_Controller *) Controller_M->ModelData.derivs)
 6037         ->TransferFcn2_CSTATE_ck[1]= Controller_X.TransferFcn2_CSTATE_ck[0];
 6038     }
 6039   
 6040     /* Derivatives for Integrator: '<S108>/Integrator' */
 6041     _rtXdot->Integrator_CSTATE_l[0] = Controller_B.Gain_n[0];
 6042     _rtXdot->Integrator_CSTATE_l[1] = Controller_B.Gain_n[1];
 6043     _rtXdot->Integrator_CSTATE_l[2] = Controller_B.Gain_n[2];
 6044   
 6045     /* Derivatives for Integrator: '<S106>/Integrator1' */
 6046     _rtXdot->Integrator1_CSTATE_b[0] = Controller_B.Sum2_g[0];
 6047     _rtXdot->Integrator1_CSTATE_b[1] = Controller_B.Sum2_g[1];
 6048     _rtXdot->Integrator1_CSTATE_b[2] = Controller_B.Sum2_g[2];
 6049   
 6050     /* Derivatives for Integrator: '<S106>/Integrator2' */
 6051     _rtXdot->Integrator2_CSTATE_h[0] = Controller_B.error_w[0];
 6052     _rtXdot->Integrator2_CSTATE_h[1] = Controller_B.error_w[1];
 6053     _rtXdot->Integrator2_CSTATE_h[2] = Controller_B.error_w[2];
 6054   }
 6055   
 6056   /* Model initialize function */
 6057   void Controller_initialize(void)
 6058   {
 6059     /* Registration code */
 6060   
 6061     /* initialize non-finites */
 6062     rt_InitInfAndNaN(sizeof(real_T));
 6063   
 6064     /* initialize real-time model */
 6065     (void) memset((void *)Controller_M, 0,
 6066                   sizeof(RT_MODEL_Controller));
 6067   
 6068     {
 6069       /* Setup solver object */
 6070       rtsiSetSimTimeStepPtr(&Controller_M->solverInfo,
 6071                             &Controller_M->Timing.simTimeStep);
 6072       rtsiSetTPtr(&Controller_M->solverInfo, &rtmGetTPtr(Controller_M));
 6073       rtsiSetStepSizePtr(&Controller_M->solverInfo,
 6074                          &Controller_M->Timing.stepSize0);
 6075       rtsiSetdXPtr(&Controller_M->solverInfo, &Controller_M->ModelData.derivs);
 6076       rtsiSetContStatesPtr(&Controller_M->solverInfo,
 6077                            &Controller_M->ModelData.contStates);
 6078       rtsiSetNumContStatesPtr(&Controller_M->solverInfo,
 6079         &Controller_M->Sizes.numContStates);
 6080       rtsiSetErrorStatusPtr(&Controller_M->solverInfo, (&rtmGetErrorStatus
 6081         (Controller_M)));
 6082       rtsiSetRTModelPtr(&Controller_M->solverInfo, Controller_M);
 6083     }
 6084   
 6085     rtsiSetSimTimeStep(&Controller_M->solverInfo, MAJOR_TIME_STEP);
 6086     Controller_M->ModelData.intgData.y = Controller_M->ModelData.odeY;
 6087     Controller_M->ModelData.intgData.f[0] = Controller_M->ModelData.odeF[0];
 6088     Controller_M->ModelData.intgData.f[1] = Controller_M->ModelData.odeF[1];
 6089     Controller_M->ModelData.intgData.f[2] = Controller_M->ModelData.odeF[2];
 6090     Controller_M->ModelData.intgData.f[3] = Controller_M->ModelData.odeF[3];
 6091     Controller_M->ModelData.contStates = ((real_T *) &Controller_X);
 6092     rtsiSetSolverData(&Controller_M->solverInfo, (void *)
 6093                       &Controller_M->ModelData.intgData);
 6094     rtsiSetSolverName(&Controller_M->solverInfo,"ode4");
 6095     rtmSetTPtr(Controller_M, &Controller_M->Timing.tArray[0]);
 6096     rtmSetTFinal(Controller_M, -1);
 6097     Controller_M->Timing.stepSize0 = 0.01;
 6098     rtmSetFirstInitCond(Controller_M, 1);
 6099   
 6100     /* Setup for data logging */
 6101     {
 6102       static RTWLogInfo rt_DataLoggingInfo;
 6103       Controller_M->rtwLogInfo = &rt_DataLoggingInfo;
 6104     }
 6105   
 6106     /* Setup for data logging */
 6107     {
 6108       rtliSetLogXSignalInfo(Controller_M->rtwLogInfo, (NULL));
 6109       rtliSetLogXSignalPtrs(Controller_M->rtwLogInfo, (NULL));
 6110       rtliSetLogT(Controller_M->rtwLogInfo, "tout");
 6111       rtliSetLogX(Controller_M->rtwLogInfo, "");
 6112       rtliSetLogXFinal(Controller_M->rtwLogInfo, "");
 6113       rtliSetSigLog(Controller_M->rtwLogInfo, "");
 6114       rtliSetLogVarNameModifier(Controller_M->rtwLogInfo, "rt_");
 6115       rtliSetLogFormat(Controller_M->rtwLogInfo, 0);
 6116       rtliSetLogMaxRows(Controller_M->rtwLogInfo, 1000);
 6117       rtliSetLogDecimation(Controller_M->rtwLogInfo, 1);
 6118   
 6119       /*
 6120        * Set pointers to the data and signal info for each output
 6121        */
 6122       {
 6123         static void * rt_LoggedOutputSignalPtrs[] = {
 6124           &Controller_Y.tau_X,
 6125           &Controller_Y.tau_Y,
 6126           &Controller_Y.tau_N,
 6127           &Controller_Y.eta_err_N,
 6128           &Controller_Y.eta_err_E,
 6129           &Controller_Y.eta_err_psi,
 6130           &Controller_Y.eta_e_N_AFF,
 6131           &Controller_Y.eta_e_E_AFF,
 6132           &Controller_Y.eta_e_psi_AFF,
 6133           &Controller_Y.nu_e_u_AFF,
 6134           &Controller_Y.nu_e_v_AFF,
 6135           &Controller_Y.nu_e_r_AFF,
 6136           &Controller_Y.d_e_x_AFF,
 6137           &Controller_Y.d_e_y_AFF,
 6138           &Controller_Y.d_e_psi_AFF,
 6139           &Controller_Y.X_com,
 6140           &Controller_Y.Y_com,
 6141           &Controller_Y.N_com,
 6142           &Controller_Y.nu_sim_u,
 6143           &Controller_Y.nu_sim_v,
 6144           &Controller_Y.nu_sim_r,
 6145           &Controller_Y.eta_sim_N,
 6146           &Controller_Y.eta_sim_E,
 6147           &Controller_Y.eta_sim_psi,
 6148           &Controller_Y.nu_d_sim_u,
 6149           &Controller_Y.nu_d_sim_v,
 6150           &Controller_Y.nu_d_sim_r,
 6151           &Controller_Y.tau_Z,
 6152           &Controller_Y.tau_K,
 6153           &Controller_Y.tau_M,
 6154           &Controller_Y.eta_e_N_NL,
 6155           &Controller_Y.eta_e_E_NL,
 6156           &Controller_Y.eta_e_psi_NL,
 6157           &Controller_Y.nu_e_u_NL,
 6158           &Controller_Y.nu_e_v_NL,
 6159           &Controller_Y.nu_e_r_NL,
 6160           &Controller_Y.d_e_x_NL,
 6161           &Controller_Y.d_e_y_NL,
 6162           &Controller_Y.d_e_psi_NL,
 6163           &Controller_Y.eta_e_N_HB,
 6164           &Controller_Y.eta_e_E_HB,
 6165           &Controller_Y.eta_e_psi_HB,
 6166           &Controller_Y.nu_e_u_HB,
 6167           &Controller_Y.nu_e_v_HB,
 6168           &Controller_Y.nu_e_r_HB,
 6169           &Controller_Y.d_e_x_HB,
 6170           &Controller_Y.d_e_y_HB,
 6171           &Controller_Y.d_e_psi_HB,
 6172           &Controller_Y.M_v_x,
 6173           &Controller_Y.M_v_y,
 6174           &Controller_Y.M_v_n,
 6175           &Controller_Y.D_v_x,
 6176           &Controller_Y.D_v_y,
 6177           &Controller_Y.D_v_n,
 6178           &Controller_Y.D_e_x,
 6179           &Controller_Y.D_e_y,
 6180           &Controller_Y.D_e_n,
 6181           &Controller_Y.K_e_x,
 6182           &Controller_Y.K_e_y,
 6183           &Controller_Y.K_e_n,
 6184           &Controller_Y.pref_x,
 6185           &Controller_Y.pref_y,
 6186           &Controller_Y.pref_n,
 6187           &Controller_Y.fref_x,
 6188           &Controller_Y.fref_y,
 6189           &Controller_Y.fref_n,
 6190           &Controller_Y.T_c_1x,
 6191           &Controller_Y.T_c_2x,
 6192           &Controller_Y.T_c_3x,
 6193           &Controller_Y.C_c_0x,
 6194           &Controller_Y.T_c_4y,
 6195           &Controller_Y.T_c_5y,
 6196           &Controller_Y.T_c_6y,
 6197           &Controller_Y.C_c_1y,
 6198           &Controller_Y.T_c_7n,
 6199           &Controller_Y.T_c_8n,
 6200           &Controller_Y.T_c_9n,
 6201           &Controller_Y.C_c_2n
 6202         };
 6203   
 6204         rtliSetLogYSignalPtrs(Controller_M->rtwLogInfo, ((LogSignalPtrsType)
 6205           rt_LoggedOutputSignalPtrs));
 6206       }
 6207   
 6208       {
 6209         static int_T rt_LoggedOutputWidths[] = {
 6210           1,
 6211           1,
 6212           1,
 6213           1,
 6214           1,
 6215           1,
 6216           1,
 6217           1,
 6218           1,
 6219           1,
 6220           1,
 6221           1,
 6222           1,
 6223           1,
 6224           1,
 6225           1,
 6226           1,
 6227           1,
 6228           1,
 6229           1,
 6230           1,
 6231           1,
 6232           1,
 6233           1,
 6234           1,
 6235           1,
 6236           1,
 6237           1,
 6238           1,
 6239           1,
 6240           1,
 6241           1,
 6242           1,
 6243           1,
 6244           1,
 6245           1,
 6246           1,
 6247           1,
 6248           1,
 6249           1,
 6250           1,
 6251           1,
 6252           1,
 6253           1,
 6254           1,
 6255           1,
 6256           1,
 6257           1,
 6258           1,
 6259           1,
 6260           1,
 6261           1,
 6262           1,
 6263           1,
 6264           1,
 6265           1,
 6266           1,
 6267           1,
 6268           1,
 6269           1,
 6270           1,
 6271           1,
 6272           1,
 6273           1,
 6274           1,
 6275           1,
 6276           1,
 6277           1,
 6278           1,
 6279           1,
 6280           1,
 6281           1,
 6282           1,
 6283           1,
 6284           1,
 6285           1,
 6286           1,
 6287           1
 6288         };
 6289   
 6290         static int_T rt_LoggedOutputNumDimensions[] = {
 6291           1,
 6292           1,
 6293           1,
 6294           1,
 6295           1,
 6296           1,
 6297           1,
 6298           1,
 6299           1,
 6300           1,
 6301           1,
 6302           1,
 6303           1,
 6304           1,
 6305           1,
 6306           1,
 6307           1,
 6308           1,
 6309           1,
 6310           1,
 6311           1,
 6312           1,
 6313           1,
 6314           1,
 6315           1,
 6316           1,
 6317           1,
 6318           1,
 6319           1,
 6320           1,
 6321           1,
 6322           1,
 6323           1,
 6324           1,
 6325           1,
 6326           1,
 6327           1,
 6328           1,
 6329           1,
 6330           1,
 6331           1,
 6332           1,
 6333           1,
 6334           1,
 6335           1,
 6336           1,
 6337           1,
 6338           1,
 6339           1,
 6340           1,
 6341           1,
 6342           1,
 6343           1,
 6344           1,
 6345           1,
 6346           1,
 6347           1,
 6348           1,
 6349           1,
 6350           1,
 6351           1,
 6352           1,
 6353           1,
 6354           1,
 6355           1,
 6356           1,
 6357           2,
 6358           2,
 6359           2,
 6360           2,
 6361           2,
 6362           2,
 6363           2,
 6364           2,
 6365           2,
 6366           2,
 6367           2,
 6368           2
 6369         };
 6370   
 6371         static int_T rt_LoggedOutputDimensions[] = {
 6372           1,
 6373           1,
 6374           1,
 6375           1,
 6376           1,
 6377           1,
 6378           1,
 6379           1,
 6380           1,
 6381           1,
 6382           1,
 6383           1,
 6384           1,
 6385           1,
 6386           1,
 6387           1,
 6388           1,
 6389           1,
 6390           1,
 6391           1,
 6392           1,
 6393           1,
 6394           1,
 6395           1,
 6396           1,
 6397           1,
 6398           1,
 6399           1,
 6400           1,
 6401           1,
 6402           1,
 6403           1,
 6404           1,
 6405           1,
 6406           1,
 6407           1,
 6408           1,
 6409           1,
 6410           1,
 6411           1,
 6412           1,
 6413           1,
 6414           1,
 6415           1,
 6416           1,
 6417           1,
 6418           1,
 6419           1,
 6420           1,
 6421           1,
 6422           1,
 6423           1,
 6424           1,
 6425           1,
 6426           1,
 6427           1,
 6428           1,
 6429           1,
 6430           1,
 6431           1,
 6432           1,
 6433           1,
 6434           1,
 6435           1,
 6436           1,
 6437           1,
 6438           1, 1,
 6439           1, 1,
 6440           1, 1,
 6441           1, 1,
 6442           1, 1,
 6443           1, 1,
 6444           1, 1,
 6445           1, 1,
 6446           1, 1,
 6447           1, 1,
 6448           1, 1,
 6449           1, 1
 6450         };
 6451   
 6452         static boolean_T rt_LoggedOutputIsVarDims[] = {
 6453           0,
 6454           0,
 6455           0,
 6456           0,
 6457           0,
 6458           0,
 6459           0,
 6460           0,
 6461           0,
 6462           0,
 6463           0,
 6464           0,
 6465           0,
 6466           0,
 6467           0,
 6468           0,
 6469           0,
 6470           0,
 6471           0,
 6472           0,
 6473           0,
 6474           0,
 6475           0,
 6476           0,
 6477           0,
 6478           0,
 6479           0,
 6480           0,
 6481           0,
 6482           0,
 6483           0,
 6484           0,
 6485           0,
 6486           0,
 6487           0,
 6488           0,
 6489           0,
 6490           0,
 6491           0,
 6492           0,
 6493           0,
 6494           0,
 6495           0,
 6496           0,
 6497           0,
 6498           0,
 6499           0,
 6500           0,
 6501           0,
 6502           0,
 6503           0,
 6504           0,
 6505           0,
 6506           0,
 6507           0,
 6508           0,
 6509           0,
 6510           0,
 6511           0,
 6512           0,
 6513           0,
 6514           0,
 6515           0,
 6516           0,
 6517           0,
 6518           0,
 6519           0,
 6520           0,
 6521           0,
 6522           0,
 6523           0,
 6524           0,
 6525           0,
 6526           0,
 6527           0,
 6528           0,
 6529           0,
 6530           0
 6531         };
 6532   
 6533         static void* rt_LoggedCurrentSignalDimensions[] = {
 6534           (NULL),
 6535           (NULL),
 6536           (NULL),
 6537           (NULL),
 6538           (NULL),
 6539           (NULL),
 6540           (NULL),
 6541           (NULL),
 6542           (NULL),
 6543           (NULL),
 6544           (NULL),
 6545           (NULL),
 6546           (NULL),
 6547           (NULL),
 6548           (NULL),
 6549           (NULL),
 6550           (NULL),
 6551           (NULL),
 6552           (NULL),
 6553           (NULL),
 6554           (NULL),
 6555           (NULL),
 6556           (NULL),
 6557           (NULL),
 6558           (NULL),
 6559           (NULL),
 6560           (NULL),
 6561           (NULL),
 6562           (NULL),
 6563           (NULL),
 6564           (NULL),
 6565           (NULL),
 6566           (NULL),
 6567           (NULL),
 6568           (NULL),
 6569           (NULL),
 6570           (NULL),
 6571           (NULL),
 6572           (NULL),
 6573           (NULL),
 6574           (NULL),
 6575           (NULL),
 6576           (NULL),
 6577           (NULL),
 6578           (NULL),
 6579           (NULL),
 6580           (NULL),
 6581           (NULL),
 6582           (NULL),
 6583           (NULL),
 6584           (NULL),
 6585           (NULL),
 6586           (NULL),
 6587           (NULL),
 6588           (NULL),
 6589           (NULL),
 6590           (NULL),
 6591           (NULL),
 6592           (NULL),
 6593           (NULL),
 6594           (NULL),
 6595           (NULL),
 6596           (NULL),
 6597           (NULL),
 6598           (NULL),
 6599           (NULL),
 6600           (NULL), (NULL),
 6601           (NULL), (NULL),
 6602           (NULL), (NULL),
 6603           (NULL), (NULL),
 6604           (NULL), (NULL),
 6605           (NULL), (NULL),
 6606           (NULL), (NULL),
 6607           (NULL), (NULL),
 6608           (NULL), (NULL),
 6609           (NULL), (NULL),
 6610           (NULL), (NULL),
 6611           (NULL), (NULL)
 6612         };
 6613   
 6614         static int_T rt_LoggedCurrentSignalDimensionsSize[] = {
 6615           4,
 6616           4,
 6617           4,
 6618           4,
 6619           4,
 6620           4,
 6621           4,
 6622           4,
 6623           4,
 6624           4,
 6625           4,
 6626           4,
 6627           4,
 6628           4,
 6629           4,
 6630           4,
 6631           4,
 6632           4,
 6633           4,
 6634           4,
 6635           4,
 6636           4,
 6637           4,
 6638           4,
 6639           4,
 6640           4,
 6641           4,
 6642           4,
 6643           4,
 6644           4,
 6645           4,
 6646           4,
 6647           4,
 6648           4,
 6649           4,
 6650           4,
 6651           4,
 6652           4,
 6653           4,
 6654           4,
 6655           4,
 6656           4,
 6657           4,
 6658           4,
 6659           4,
 6660           4,
 6661           4,
 6662           4,
 6663           4,
 6664           4,
 6665           4,
 6666           4,
 6667           4,
 6668           4,
 6669           4,
 6670           4,
 6671           4,
 6672           4,
 6673           4,
 6674           4,
 6675           4,
 6676           4,
 6677           4,
 6678           4,
 6679           4,
 6680           4,
 6681           4, 4,
 6682           4, 4,
 6683           4, 4,
 6684           4, 4,
 6685           4, 4,
 6686           4, 4,
 6687           4, 4,
 6688           4, 4,
 6689           4, 4,
 6690           4, 4,
 6691           4, 4,
 6692           4, 4
 6693         };
 6694   
 6695         static BuiltInDTypeId rt_LoggedOutputDataTypeIds[] = {
 6696           SS_DOUBLE,
 6697           SS_DOUBLE,
 6698           SS_DOUBLE,
 6699           SS_DOUBLE,
 6700           SS_DOUBLE,
 6701           SS_DOUBLE,
 6702           SS_DOUBLE,
 6703           SS_DOUBLE,
 6704           SS_DOUBLE,
 6705           SS_DOUBLE,
 6706           SS_DOUBLE,
 6707           SS_DOUBLE,
 6708           SS_DOUBLE,
 6709           SS_DOUBLE,
 6710           SS_DOUBLE,
 6711           SS_DOUBLE,
 6712           SS_DOUBLE,
 6713           SS_DOUBLE,
 6714           SS_DOUBLE,
 6715           SS_DOUBLE,
 6716           SS_DOUBLE,
 6717           SS_DOUBLE,
 6718           SS_DOUBLE,
 6719           SS_DOUBLE,
 6720           SS_DOUBLE,
 6721           SS_DOUBLE,
 6722           SS_DOUBLE,
 6723           SS_DOUBLE,
 6724           SS_DOUBLE,
 6725           SS_DOUBLE,
 6726           SS_DOUBLE,
 6727           SS_DOUBLE,
 6728           SS_DOUBLE,
 6729           SS_DOUBLE,
 6730           SS_DOUBLE,
 6731           SS_DOUBLE,
 6732           SS_DOUBLE,
 6733           SS_DOUBLE,
 6734           SS_DOUBLE,
 6735           SS_DOUBLE,
 6736           SS_DOUBLE,
 6737           SS_DOUBLE,
 6738           SS_DOUBLE,
 6739           SS_DOUBLE,
 6740           SS_DOUBLE,
 6741           SS_DOUBLE,
 6742           SS_DOUBLE,
 6743           SS_DOUBLE,
 6744           SS_DOUBLE,
 6745           SS_DOUBLE,
 6746           SS_DOUBLE,
 6747           SS_DOUBLE,
 6748           SS_DOUBLE,
 6749           SS_DOUBLE,
 6750           SS_DOUBLE,
 6751           SS_DOUBLE,
 6752           SS_DOUBLE,
 6753           SS_DOUBLE,
 6754           SS_DOUBLE,
 6755           SS_DOUBLE,
 6756           SS_DOUBLE,
 6757           SS_DOUBLE,
 6758           SS_DOUBLE,
 6759           SS_DOUBLE,
 6760           SS_DOUBLE,
 6761           SS_DOUBLE,
 6762           SS_DOUBLE,
 6763           SS_DOUBLE,
 6764           SS_DOUBLE,
 6765           SS_DOUBLE,
 6766           SS_DOUBLE,
 6767           SS_DOUBLE,
 6768           SS_DOUBLE,
 6769           SS_DOUBLE,
 6770           SS_DOUBLE,
 6771           SS_DOUBLE,
 6772           SS_DOUBLE,
 6773           SS_DOUBLE
 6774         };
 6775   
 6776         static int_T rt_LoggedOutputComplexSignals[] = {
 6777           0,
 6778           0,
 6779           0,
 6780           0,
 6781           0,
 6782           0,
 6783           0,
 6784           0,
 6785           0,
 6786           0,
 6787           0,
 6788           0,
 6789           0,
 6790           0,
 6791           0,
 6792           0,
 6793           0,
 6794           0,
 6795           0,
 6796           0,
 6797           0,
 6798           0,
 6799           0,
 6800           0,
 6801           0,
 6802           0,
 6803           0,
 6804           0,
 6805           0,
 6806           0,
 6807           0,
 6808           0,
 6809           0,
 6810           0,
 6811           0,
 6812           0,
 6813           0,
 6814           0,
 6815           0,
 6816           0,
 6817           0,
 6818           0,
 6819           0,
 6820           0,
 6821           0,
 6822           0,
 6823           0,
 6824           0,
 6825           0,
 6826           0,
 6827           0,
 6828           0,
 6829           0,
 6830           0,
 6831           0,
 6832           0,
 6833           0,
 6834           0,
 6835           0,
 6836           0,
 6837           0,
 6838           0,
 6839           0,
 6840           0,
 6841           0,
 6842           0,
 6843           0,
 6844           0,
 6845           0,
 6846           0,
 6847           0,
 6848           0,
 6849           0,
 6850           0,
 6851           0,
 6852           0,
 6853           0,
 6854           0
 6855         };
 6856   
 6857         static const char_T *rt_LoggedOutputLabels[] = {
 6858           "",
 6859           "",
 6860           "",
 6861           "",
 6862           "",
 6863           "",
 6864           "",
 6865           "",
 6866           "",
 6867           "",
 6868           "",
 6869           "",
 6870           "",
 6871           "",
 6872           "",
 6873           "",
 6874           "",
 6875           "",
 6876           "",
 6877           "",
 6878           "",
 6879           "",
 6880           "",
 6881           "",
 6882           "",
 6883           "",
 6884           "",
 6885           "",
 6886           "",
 6887           "",
 6888           "",
 6889           "",
 6890           "",
 6891           "",
 6892           "",
 6893           "",
 6894           "",
 6895           "",
 6896           "",
 6897           "",
 6898           "",
 6899           "",
 6900           "",
 6901           "",
 6902           "",
 6903           "",
 6904           "",
 6905           "",
 6906           "",
 6907           "",
 6908           "",
 6909           "",
 6910           "",
 6911           "",
 6912           "",
 6913           "",
 6914           "",
 6915           "",
 6916           "",
 6917           "",
 6918           "",
 6919           "",
 6920           "",
 6921           "",
 6922           "",
 6923           "",
 6924           "",
 6925           "",
 6926           "",
 6927           "",
 6928           "",
 6929           "",
 6930           "",
 6931           "",
 6932           "",
 6933           "",
 6934           "",
 6935           "" };
 6936   
 6937         static const char_T *rt_LoggedOutputBlockNames[] = {
 6938           "Controller/tau_X",
 6939           "Controller/tau_Y",
 6940           "Controller/tau_N",
 6941           "Controller/eta_err_N",
 6942           "Controller/eta_err_E",
 6943           "Controller/eta_err_psi",
 6944           "Controller/eta_e_N_AFF",
 6945           "Controller/eta_e_E_AFF",
 6946           "Controller/eta_e_psi_AFF",
 6947           "Controller/nu_e_u_AFF",
 6948           "Controller/nu_e_v_AFF",
 6949           "Controller/nu_e_r_AFF",
 6950           "Controller/d_e_x_AFF",
 6951           "Controller/d_e_y_AFF",
 6952           "Controller/d_e_psi_AFF",
 6953           "Controller/X_com",
 6954           "Controller/Y_com",
 6955           "Controller/N_com",
 6956           "Controller/nu_sim_u",
 6957           "Controller/nu_sim_v",
 6958           "Controller/nu_sim_r",
 6959           "Controller/eta_sim_N",
 6960           "Controller/eta_sim_E",
 6961           "Controller/eta_sim_psi",
 6962           "Controller/nu_d_sim_u",
 6963           "Controller/nu_d_sim_v",
 6964           "Controller/nu_d_sim_r",
 6965           "Controller/tau_Z",
 6966           "Controller/tau_K",
 6967           "Controller/tau_M",
 6968           "Controller/eta_e_N_NL",
 6969           "Controller/eta_e_E_NL",
 6970           "Controller/eta_e_psi_NL",
 6971           "Controller/nu_e_u_NL",
 6972           "Controller/nu_e_v_NL",
 6973           "Controller/nu_e_r_NL",
 6974           "Controller/d_e_x_NL",
 6975           "Controller/d_e_y_NL",
 6976           "Controller/d_e_psi_NL",
 6977           "Controller/eta_e_N_HB",
 6978           "Controller/eta_e_E_HB",
 6979           "Controller/eta_e_psi_HB",
 6980           "Controller/nu_e_u_HB",
 6981           "Controller/nu_e_v_HB",
 6982           "Controller/nu_e_r_HB",
 6983           "Controller/d_e_x_HB",
 6984           "Controller/d_e_y_HB",
 6985           "Controller/d_e_psi_HB",
 6986           "Controller/M_v_x",
 6987           "Controller/M_v_y",
 6988           "Controller/M_v_n",
 6989           "Controller/D_v_x",
 6990           "Controller/D_v_y",
 6991           "Controller/D_v_n",
 6992           "Controller/D_e_x",
 6993           "Controller/D_e_y",
 6994           "Controller/D_e_n",
 6995           "Controller/K_e_x",
 6996           "Controller/K_e_y",
 6997           "Controller/K_e_n",
 6998           "Controller/pref_x",
 6999           "Controller/pref_y",
 7000           "Controller/pref_n",
 7001           "Controller/fref_x",
 7002           "Controller/fref_y",
 7003           "Controller/fref_n",
 7004           "Controller/T_c_1x",
 7005           "Controller/T_c_2x",
 7006           "Controller/T_c_3x",
 7007           "Controller/C_c_0x",
 7008           "Controller/T_c_4y",
 7009           "Controller/T_c_5y",
 7010           "Controller/T_c_6y",
 7011           "Controller/C_c_1y",
 7012           "Controller/T_c_7n",
 7013           "Controller/T_c_8n",
 7014           "Controller/T_c_9n",
 7015           "Controller/C_c_2n" };
 7016   
 7017         static RTWLogDataTypeConvert rt_RTWLogDataTypeConvert[] = {
 7018           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7019   
 7020           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7021   
 7022           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7023   
 7024           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7025   
 7026           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7027   
 7028           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7029   
 7030           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7031   
 7032           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7033   
 7034           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7035   
 7036           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7037   
 7038           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7039   
 7040           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7041   
 7042           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7043   
 7044           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7045   
 7046           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7047   
 7048           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7049   
 7050           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7051   
 7052           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7053   
 7054           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7055   
 7056           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7057   
 7058           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7059   
 7060           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7061   
 7062           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7063   
 7064           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7065   
 7066           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7067   
 7068           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7069   
 7070           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7071   
 7072           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7073   
 7074           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7075   
 7076           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7077   
 7078           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7079   
 7080           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7081   
 7082           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7083   
 7084           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7085   
 7086           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7087   
 7088           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7089   
 7090           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7091   
 7092           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7093   
 7094           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7095   
 7096           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7097   
 7098           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7099   
 7100           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7101   
 7102           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7103   
 7104           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7105   
 7106           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7107   
 7108           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7109   
 7110           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7111   
 7112           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7113   
 7114           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7115   
 7116           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7117   
 7118           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7119   
 7120           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7121   
 7122           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7123   
 7124           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7125   
 7126           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7127   
 7128           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7129   
 7130           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7131   
 7132           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7133   
 7134           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7135   
 7136           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7137   
 7138           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7139   
 7140           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7141   
 7142           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7143   
 7144           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7145   
 7146           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7147   
 7148           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7149   
 7150           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7151   
 7152           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7153   
 7154           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7155   
 7156           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7157   
 7158           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7159   
 7160           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7161   
 7162           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7163   
 7164           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7165   
 7166           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7167   
 7168           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7169   
 7170           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 },
 7171   
 7172           { 0, SS_DOUBLE, SS_DOUBLE, 0, 0, 0, 1.0, 0, 0.0 }
 7173         };
 7174   
 7175         static RTWLogSignalInfo rt_LoggedOutputSignalInfo[] = {
 7176           {
 7177             78,
 7178             rt_LoggedOutputWidths,
 7179             rt_LoggedOutputNumDimensions,
 7180             rt_LoggedOutputDimensions,
 7181             rt_LoggedOutputIsVarDims,
 7182             rt_LoggedCurrentSignalDimensions,
 7183             rt_LoggedCurrentSignalDimensionsSize,
 7184             rt_LoggedOutputDataTypeIds,
 7185             rt_LoggedOutputComplexSignals,
 7186             (NULL),
 7187   
 7188             { rt_LoggedOutputLabels },
 7189             (NULL),
 7190             (NULL),
 7191             (NULL),
 7192   
 7193             { rt_LoggedOutputBlockNames },
 7194   
 7195             { (NULL) },
 7196             (NULL),
 7197             rt_RTWLogDataTypeConvert
 7198           }
 7199         };
 7200   
 7201         rtliSetLogYSignalInfo(Controller_M->rtwLogInfo, rt_LoggedOutputSignalInfo);
 7202   
 7203         /* set currSigDims field */
 7204         rt_LoggedCurrentSignalDimensions[0] = &rt_LoggedOutputWidths[0];
 7205         rt_LoggedCurrentSignalDimensions[1] = &rt_LoggedOutputWidths[1];
 7206         rt_LoggedCurrentSignalDimensions[2] = &rt_LoggedOutputWidths[2];
 7207         rt_LoggedCurrentSignalDimensions[3] = &rt_LoggedOutputWidths[3];
 7208         rt_LoggedCurrentSignalDimensions[4] = &rt_LoggedOutputWidths[4];
 7209         rt_LoggedCurrentSignalDimensions[5] = &rt_LoggedOutputWidths[5];
 7210         rt_LoggedCurrentSignalDimensions[6] = &rt_LoggedOutputWidths[6];
 7211         rt_LoggedCurrentSignalDimensions[7] = &rt_LoggedOutputWidths[7];
 7212         rt_LoggedCurrentSignalDimensions[8] = &rt_LoggedOutputWidths[8];
 7213         rt_LoggedCurrentSignalDimensions[9] = &rt_LoggedOutputWidths[9];
 7214         rt_LoggedCurrentSignalDimensions[10] = &rt_LoggedOutputWidths[10];
 7215         rt_LoggedCurrentSignalDimensions[11] = &rt_LoggedOutputWidths[11];
 7216         rt_LoggedCurrentSignalDimensions[12] = &rt_LoggedOutputWidths[12];
 7217         rt_LoggedCurrentSignalDimensions[13] = &rt_LoggedOutputWidths[13];
 7218         rt_LoggedCurrentSignalDimensions[14] = &rt_LoggedOutputWidths[14];
 7219         rt_LoggedCurrentSignalDimensions[15] = &rt_LoggedOutputWidths[15];
 7220         rt_LoggedCurrentSignalDimensions[16] = &rt_LoggedOutputWidths[16];
 7221         rt_LoggedCurrentSignalDimensions[17] = &rt_LoggedOutputWidths[17];
 7222         rt_LoggedCurrentSignalDimensions[18] = &rt_LoggedOutputWidths[18];
 7223         rt_LoggedCurrentSignalDimensions[19] = &rt_LoggedOutputWidths[19];
 7224         rt_LoggedCurrentSignalDimensions[20] = &rt_LoggedOutputWidths[20];
 7225         rt_LoggedCurrentSignalDimensions[21] = &rt_LoggedOutputWidths[21];
 7226         rt_LoggedCurrentSignalDimensions[22] = &rt_LoggedOutputWidths[22];
 7227         rt_LoggedCurrentSignalDimensions[23] = &rt_LoggedOutputWidths[23];
 7228         rt_LoggedCurrentSignalDimensions[24] = &rt_LoggedOutputWidths[24];
 7229         rt_LoggedCurrentSignalDimensions[25] = &rt_LoggedOutputWidths[25];
 7230         rt_LoggedCurrentSignalDimensions[26] = &rt_LoggedOutputWidths[26];
 7231         rt_LoggedCurrentSignalDimensions[27] = &rt_LoggedOutputWidths[27];
 7232         rt_LoggedCurrentSignalDimensions[28] = &rt_LoggedOutputWidths[28];
 7233         rt_LoggedCurrentSignalDimensions[29] = &rt_LoggedOutputWidths[29];
 7234         rt_LoggedCurrentSignalDimensions[30] = &rt_LoggedOutputWidths[30];
 7235         rt_LoggedCurrentSignalDimensions[31] = &rt_LoggedOutputWidths[31];
 7236         rt_LoggedCurrentSignalDimensions[32] = &rt_LoggedOutputWidths[32];
 7237         rt_LoggedCurrentSignalDimensions[33] = &rt_LoggedOutputWidths[33];
 7238         rt_LoggedCurrentSignalDimensions[34] = &rt_LoggedOutputWidths[34];
 7239         rt_LoggedCurrentSignalDimensions[35] = &rt_LoggedOutputWidths[35];
 7240         rt_LoggedCurrentSignalDimensions[36] = &rt_LoggedOutputWidths[36];
 7241         rt_LoggedCurrentSignalDimensions[37] = &rt_LoggedOutputWidths[37];
 7242         rt_LoggedCurrentSignalDimensions[38] = &rt_LoggedOutputWidths[38];
 7243         rt_LoggedCurrentSignalDimensions[39] = &rt_LoggedOutputWidths[39];
 7244         rt_LoggedCurrentSignalDimensions[40] = &rt_LoggedOutputWidths[40];
 7245         rt_LoggedCurrentSignalDimensions[41] = &rt_LoggedOutputWidths[41];
 7246         rt_LoggedCurrentSignalDimensions[42] = &rt_LoggedOutputWidths[42];
 7247         rt_LoggedCurrentSignalDimensions[43] = &rt_LoggedOutputWidths[43];
 7248         rt_LoggedCurrentSignalDimensions[44] = &rt_LoggedOutputWidths[44];
 7249         rt_LoggedCurrentSignalDimensions[45] = &rt_LoggedOutputWidths[45];
 7250         rt_LoggedCurrentSignalDimensions[46] = &rt_LoggedOutputWidths[46];
 7251         rt_LoggedCurrentSignalDimensions[47] = &rt_LoggedOutputWidths[47];
 7252         rt_LoggedCurrentSignalDimensions[48] = &rt_LoggedOutputWidths[48];
 7253         rt_LoggedCurrentSignalDimensions[49] = &rt_LoggedOutputWidths[49];
 7254         rt_LoggedCurrentSignalDimensions[50] = &rt_LoggedOutputWidths[50];
 7255         rt_LoggedCurrentSignalDimensions[51] = &rt_LoggedOutputWidths[51];
 7256         rt_LoggedCurrentSignalDimensions[52] = &rt_LoggedOutputWidths[52];
 7257         rt_LoggedCurrentSignalDimensions[53] = &rt_LoggedOutputWidths[53];
 7258         rt_LoggedCurrentSignalDimensions[54] = &rt_LoggedOutputWidths[54];
 7259         rt_LoggedCurrentSignalDimensions[55] = &rt_LoggedOutputWidths[55];
 7260         rt_LoggedCurrentSignalDimensions[56] = &rt_LoggedOutputWidths[56];
 7261         rt_LoggedCurrentSignalDimensions[57] = &rt_LoggedOutputWidths[57];
 7262         rt_LoggedCurrentSignalDimensions[58] = &rt_LoggedOutputWidths[58];
 7263         rt_LoggedCurrentSignalDimensions[59] = &rt_LoggedOutputWidths[59];
 7264         rt_LoggedCurrentSignalDimensions[60] = &rt_LoggedOutputWidths[60];
 7265         rt_LoggedCurrentSignalDimensions[61] = &rt_LoggedOutputWidths[61];
 7266         rt_LoggedCurrentSignalDimensions[62] = &rt_LoggedOutputWidths[62];
 7267         rt_LoggedCurrentSignalDimensions[63] = &rt_LoggedOutputWidths[63];
 7268         rt_LoggedCurrentSignalDimensions[64] = &rt_LoggedOutputWidths[64];
 7269         rt_LoggedCurrentSignalDimensions[65] = &rt_LoggedOutputWidths[65];
 7270         rt_LoggedCurrentSignalDimensions[66] = &rt_LoggedOutputWidths[66];
 7271         rt_LoggedCurrentSignalDimensions[67] = &rt_LoggedOutputWidths[66];
 7272         rt_LoggedCurrentSignalDimensions[68] = &rt_LoggedOutputWidths[67];
 7273         rt_LoggedCurrentSignalDimensions[69] = &rt_LoggedOutputWidths[67];
 7274         rt_LoggedCurrentSignalDimensions[70] = &rt_LoggedOutputWidths[68];
 7275         rt_LoggedCurrentSignalDimensions[71] = &rt_LoggedOutputWidths[68];
 7276         rt_LoggedCurrentSignalDimensions[72] = &rt_LoggedOutputWidths[69];
 7277         rt_LoggedCurrentSignalDimensions[73] = &rt_LoggedOutputWidths[69];
 7278         rt_LoggedCurrentSignalDimensions[74] = &rt_LoggedOutputWidths[70];
 7279         rt_LoggedCurrentSignalDimensions[75] = &rt_LoggedOutputWidths[70];
 7280         rt_LoggedCurrentSignalDimensions[76] = &rt_LoggedOutputWidths[71];
 7281         rt_LoggedCurrentSignalDimensions[77] = &rt_LoggedOutputWidths[71];
 7282         rt_LoggedCurrentSignalDimensions[78] = &rt_LoggedOutputWidths[72];
 7283         rt_LoggedCurrentSignalDimensions[79] = &rt_LoggedOutputWidths[72];
 7284         rt_LoggedCurrentSignalDimensions[80] = &rt_LoggedOutputWidths[73];
 7285         rt_LoggedCurrentSignalDimensions[81] = &rt_LoggedOutputWidths[73];
 7286         rt_LoggedCurrentSignalDimensions[82] = &rt_LoggedOutputWidths[74];
 7287         rt_LoggedCurrentSignalDimensions[83] = &rt_LoggedOutputWidths[74];
 7288         rt_LoggedCurrentSignalDimensions[84] = &rt_LoggedOutputWidths[75];
 7289         rt_LoggedCurrentSignalDimensions[85] = &rt_LoggedOutputWidths[75];
 7290         rt_LoggedCurrentSignalDimensions[86] = &rt_LoggedOutputWidths[76];
 7291         rt_LoggedCurrentSignalDimensions[87] = &rt_LoggedOutputWidths[76];
 7292         rt_LoggedCurrentSignalDimensions[88] = &rt_LoggedOutputWidths[77];
 7293         rt_LoggedCurrentSignalDimensions[89] = &rt_LoggedOutputWidths[77];
 7294       }
 7295   
 7296       rtliSetLogY(Controller_M->rtwLogInfo, "yout");
 7297     }
 7298   
 7299     /* block I/O */
 7300     (void) memset(((void *) &Controller_B), 0,
 7301                   sizeof(BlockIO_Controller));
 7302   
 7303     /* states (continuous) */
 7304     {
 7305       (void) memset((void *)&Controller_X, 0,
 7306                     sizeof(ContinuousStates_Controller));
 7307     }
 7308   
 7309     /* states (dwork) */
 7310     (void) memset((void *)&Controller_DWork, 0,
 7311                   sizeof(D_Work_Controller));
 7312   
 7313     /* external inputs */
 7314     (void) memset((void *)&Controller_U, 0,
 7315                   sizeof(ExternalInputs_Controller));
 7316   
 7317     /* external outputs */
 7318     (void) memset((void *)&Controller_Y, 0,
 7319                   sizeof(ExternalOutputs_Controller));
 7320   
 7321     /* Matfile logging */
 7322     rt_StartDataLoggingWithStartTime(Controller_M->rtwLogInfo, 0.0, rtmGetTFinal
 7323       (Controller_M), Controller_M->Timing.stepSize0, (&rtmGetErrorStatus
 7324       (Controller_M)));
 7325   
 7326     {
 7327       uint32_T tseed;
 7328       int32_T r;
 7329       int32_T t;
 7330       real_T tmp;
 7331   
 7332       /* Start for TransportDelay: '<S93>/Transport Delay' */
 7333       {
 7334         real_T *pBuffer = &Controller_DWork.TransportDelay_RWORK.TUbufferArea[0];
 7335   
 7336         {
 7337           int_T i1;
 7338           int_T *iw_Tail = &Controller_DWork.TransportDelay_IWORK.Tail[0];
 7339           int_T *iw_Head = &Controller_DWork.TransportDelay_IWORK.Head[0];
 7340           int_T *iw_Last = &Controller_DWork.TransportDelay_IWORK.Last[0];
 7341           int_T *iw_CircularBufSize =
 7342             &Controller_DWork.TransportDelay_IWORK.CircularBufSize[0];
 7343           void **pw_TUbufferPtrs =
 7344             &Controller_DWork.TransportDelay_PWORK.TUbufferPtrs[0];
 7345           for (i1=0; i1 < 3; i1++) {
 7346             iw_Tail[i1] = 0;
 7347             iw_Head[i1] = 0;
 7348             iw_Last[i1] = 0;
 7349             iw_CircularBufSize[i1] = 1024;
 7350             pBuffer[0] = Controller_P.TransportDelay_InitOutput;
 7351             pBuffer[1024] = Controller_M->Timing.t[0];
 7352             pw_TUbufferPtrs[i1] = (void *) &pBuffer[0];
 7353             pw_TUbufferPtrs[i1+3] = (void *) &pBuffer[1024];
 7354             pBuffer += 2048;
 7355           }
 7356         }
 7357       }
 7358   
 7359       /* Start for RandomNumber: '<S58>/White Noise' */
 7360       tmp = floor(Controller_P.WhiteNoise_Seed);
 7361       if (rtIsNaN(tmp) || rtIsInf(tmp)) {
 7362         tmp = 0.0;
 7363       } else {
 7364         tmp = fmod(tmp, 4.294967296E+9);
 7365       }
 7366   
 7367       tseed = tmp < 0.0 ? (uint32_T)-(int32_T)(uint32_T)-tmp : (uint32_T)tmp;
 7368       r = (int32_T)(tseed >> 16U);
 7369       t = (int32_T)(tseed & 32768U);
 7370       tseed = ((((tseed - ((uint32_T)r << 16U)) + (uint32_T)t) << 16U) + (uint32_T)
 7371                t) + (uint32_T)r;
 7372       if (tseed < 1U) {
 7373         tseed = 1144108930U;
 7374       } else {
 7375         if (tseed > 2147483646U) {
 7376           tseed = 2147483646U;
 7377         }
 7378       }
 7379   
 7380       Controller_DWork.RandSeed = tseed;
 7381       Controller_DWork.NextOutput = rt_nrand_Upu32_Yd_f_pw_snf
 7382         (&Controller_DWork.RandSeed) * Controller_P.WhiteNoise_StdDev +
 7383         Controller_P.WhiteNoise_Mean;
 7384   
 7385       /* End of Start for RandomNumber: '<S58>/White Noise' */
 7386   
 7387       /* Start for RandomNumber: '<S63>/White Noise' */
 7388       tmp = floor(Controller_P.WhiteNoise_Seed_n);
 7389       if (rtIsNaN(tmp) || rtIsInf(tmp)) {
 7390         tmp = 0.0;
 7391       } else {
 7392         tmp = fmod(tmp, 4.294967296E+9);
 7393       }
 7394   
 7395       tseed = tmp < 0.0 ? (uint32_T)-(int32_T)(uint32_T)-tmp : (uint32_T)tmp;
 7396       r = (int32_T)(tseed >> 16U);
 7397       t = (int32_T)(tseed & 32768U);
 7398       tseed = ((((tseed - ((uint32_T)r << 16U)) + (uint32_T)t) << 16U) + (uint32_T)
 7399                t) + (uint32_T)r;
 7400       if (tseed < 1U) {
 7401         tseed = 1144108930U;
 7402       } else {
 7403         if (tseed > 2147483646U) {
 7404           tseed = 2147483646U;
 7405         }
 7406       }
 7407   
 7408       Controller_DWork.RandSeed_f = tseed;
 7409       Controller_DWork.NextOutput_k = rt_nrand_Upu32_Yd_f_pw_snf
 7410         (&Controller_DWork.RandSeed_f) * Controller_P.WhiteNoise_StdDev_b +
 7411         Controller_P.WhiteNoise_Mean_f;
 7412   
 7413       /* End of Start for RandomNumber: '<S63>/White Noise' */
 7414   
 7415       /* Start for RandomNumber: '<S68>/White Noise' */
 7416       tmp = floor(Controller_P.WhiteNoise_Seed_k);
 7417       if (rtIsNaN(tmp) || rtIsInf(tmp)) {
 7418         tmp = 0.0;
 7419       } else {
 7420         tmp = fmod(tmp, 4.294967296E+9);
 7421       }
 7422   
 7423       tseed = tmp < 0.0 ? (uint32_T)-(int32_T)(uint32_T)-tmp : (uint32_T)tmp;
 7424       r = (int32_T)(tseed >> 16U);
 7425       t = (int32_T)(tseed & 32768U);
 7426       tseed = ((((tseed - ((uint32_T)r << 16U)) + (uint32_T)t) << 16U) + (uint32_T)
 7427                t) + (uint32_T)r;
 7428       if (tseed < 1U) {
 7429         tseed = 1144108930U;
 7430       } else {
 7431         if (tseed > 2147483646U) {
 7432           tseed = 2147483646U;
 7433         }
 7434       }
 7435   
 7436       Controller_DWork.RandSeed_p = tseed;
 7437       Controller_DWork.NextOutput_f = rt_nrand_Upu32_Yd_f_pw_snf
 7438         (&Controller_DWork.RandSeed_p) * Controller_P.WhiteNoise_StdDev_h +
 7439         Controller_P.WhiteNoise_Mean_g;
 7440   
 7441       /* End of Start for RandomNumber: '<S68>/White Noise' */
 7442   
 7443       /* Start for RandomNumber: '<S73>/White Noise' */
 7444       tmp = floor(Controller_P.WhiteNoise_Seed_no);
 7445       if (rtIsNaN(tmp) || rtIsInf(tmp)) {
 7446         tmp = 0.0;
 7447       } else {
 7448         tmp = fmod(tmp, 4.294967296E+9);
 7449       }
 7450   
 7451       tseed = tmp < 0.0 ? (uint32_T)-(int32_T)(uint32_T)-tmp : (uint32_T)tmp;
 7452       r = (int32_T)(tseed >> 16U);
 7453       t = (int32_T)(tseed & 32768U);
 7454       tseed = ((((tseed - ((uint32_T)r << 16U)) + (uint32_T)t) << 16U) + (uint32_T)
 7455                t) + (uint32_T)r;
 7456       if (tseed < 1U) {
 7457         tseed = 1144108930U;
 7458       } else {
 7459         if (tseed > 2147483646U) {
 7460           tseed = 2147483646U;
 7461         }
 7462       }
 7463   
 7464       Controller_DWork.RandSeed_a = tseed;
 7465       Controller_DWork.NextOutput_c = rt_nrand_Upu32_Yd_f_pw_snf
 7466         (&Controller_DWork.RandSeed_a) * Controller_P.WhiteNoise_StdDev_d +
 7467         Controller_P.WhiteNoise_Mean_j;
 7468   
 7469       /* End of Start for RandomNumber: '<S73>/White Noise' */
 7470   
 7471       /* Start for RandomNumber: '<S75>/White Noise' */
 7472       tmp = floor(Controller_P.WhiteNoise_Seed_kt[0]);
 7473       if (rtIsNaN(tmp) || rtIsInf(tmp)) {
 7474         tmp = 0.0;
 7475       } else {
 7476         tmp = fmod(tmp, 4.294967296E+9);
 7477       }
 7478   
 7479       tseed = tmp < 0.0 ? (uint32_T)-(int32_T)(uint32_T)-tmp : (uint32_T)tmp;
 7480       r = (int32_T)(tseed >> 16U);
 7481       t = (int32_T)(tseed & 32768U);
 7482       tseed = ((((tseed - ((uint32_T)r << 16U)) + (uint32_T)t) << 16U) + (uint32_T)
 7483                t) + (uint32_T)r;
 7484       if (tseed < 1U) {
 7485         tseed = 1144108930U;
 7486       } else {
 7487         if (tseed > 2147483646U) {
 7488           tseed = 2147483646U;
 7489         }
 7490       }
 7491   
 7492       Controller_DWork.RandSeed_fl[0] = tseed;
 7493       Controller_DWork.NextOutput_m[0] = rt_nrand_Upu32_Yd_f_pw_snf
 7494         (&Controller_DWork.RandSeed_fl[0]) * Controller_P.WhiteNoise_StdDev_j +
 7495         Controller_P.WhiteNoise_Mean_p;
 7496       tmp = floor(Controller_P.WhiteNoise_Seed_kt[1]);
 7497       if (rtIsNaN(tmp) || rtIsInf(tmp)) {
 7498         tmp = 0.0;
 7499       } else {
 7500         tmp = fmod(tmp, 4.294967296E+9);
 7501       }
 7502   
 7503       tseed = tmp < 0.0 ? (uint32_T)-(int32_T)(uint32_T)-tmp : (uint32_T)tmp;
 7504       r = (int32_T)(tseed >> 16U);
 7505       t = (int32_T)(tseed & 32768U);
 7506       tseed = ((((tseed - ((uint32_T)r << 16U)) + (uint32_T)t) << 16U) + (uint32_T)
 7507                t) + (uint32_T)r;
 7508       if (tseed < 1U) {
 7509         tseed = 1144108930U;
 7510       } else {
 7511         if (tseed > 2147483646U) {
 7512           tseed = 2147483646U;
 7513         }
 7514       }
 7515   
 7516       Controller_DWork.RandSeed_fl[1] = tseed;
 7517       Controller_DWork.NextOutput_m[1] = rt_nrand_Upu32_Yd_f_pw_snf
 7518         (&Controller_DWork.RandSeed_fl[1]) * Controller_P.WhiteNoise_StdDev_j +
 7519         Controller_P.WhiteNoise_Mean_p;
 7520       tmp = floor(Controller_P.WhiteNoise_Seed_kt[2]);
 7521       if (rtIsNaN(tmp) || rtIsInf(tmp)) {
 7522         tmp = 0.0;
 7523       } else {
 7524         tmp = fmod(tmp, 4.294967296E+9);
 7525       }
 7526   
 7527       tseed = tmp < 0.0 ? (uint32_T)-(int32_T)(uint32_T)-tmp : (uint32_T)tmp;
 7528       r = (int32_T)(tseed >> 16U);
 7529       t = (int32_T)(tseed & 32768U);
 7530       tseed = ((((tseed - ((uint32_T)r << 16U)) + (uint32_T)t) << 16U) + (uint32_T)
 7531                t) + (uint32_T)r;
 7532       if (tseed < 1U) {
 7533         tseed = 1144108930U;
 7534       } else {
 7535         if (tseed > 2147483646U) {
 7536           tseed = 2147483646U;
 7537         }
 7538       }
 7539   
 7540       Controller_DWork.RandSeed_fl[2] = tseed;
 7541       Controller_DWork.NextOutput_m[2] = rt_nrand_Upu32_Yd_f_pw_snf
 7542         (&Controller_DWork.RandSeed_fl[2]) * Controller_P.WhiteNoise_StdDev_j +
 7543         Controller_P.WhiteNoise_Mean_p;
 7544   
 7545       /* End of Start for RandomNumber: '<S75>/White Noise' */
 7546     }
 7547   
 7548     Controller_PrevZCSigState.Integrator1_Reset_ZCE = UNINITIALIZED_ZCSIG;
 7549     Controller_PrevZCSigState.Integrator2_Reset_ZCE = UNINITIALIZED_ZCSIG;
 7550     Controller_PrevZCSigState.Integrator3_Reset_ZCE = UNINITIALIZED_ZCSIG;
 7551   
 7552     {
 7553       int32_T i;
 7554   
 7555       /* InitializeConditions for Integrator: '<S14>/Integrator' */
 7556       Controller_X.Integrator_CSTATE[0] = Controller_P.Integrator_IC;
 7557       Controller_X.Integrator_CSTATE[1] = Controller_P.Integrator_IC;
 7558       Controller_X.Integrator_CSTATE[2] = Controller_P.Integrator_IC;
 7559   
 7560       /* InitializeConditions for Integrator: '<S104>/Integrator1' */
 7561       Controller_X.Integrator1_CSTATE[0] = Controller_P.Integrator1_IC[0];
 7562       Controller_X.Integrator1_CSTATE[1] = Controller_P.Integrator1_IC[1];
 7563       Controller_X.Integrator1_CSTATE[2] = Controller_P.Integrator1_IC[2];
 7564   
 7565       /* InitializeConditions for TransferFcn: '<S77>/Transfer Fcn2' */
 7566       Controller_X.TransferFcn2_CSTATE[0] = 0.0;
 7567       Controller_X.TransferFcn2_CSTATE[1] = 0.0;
 7568   
 7569       /* InitializeConditions for TransferFcn: '<S77>/Transfer Fcn1' */
 7570       Controller_X.TransferFcn1_CSTATE[0] = 0.0;
 7571       Controller_X.TransferFcn1_CSTATE[1] = 0.0;
 7572   
 7573       /* InitializeConditions for TransferFcn: '<S77>/Transfer Fcn' */
 7574       Controller_X.TransferFcn_CSTATE[0] = 0.0;
 7575       Controller_X.TransferFcn_CSTATE[1] = 0.0;
 7576   
 7577       /* InitializeConditions for Integrator: '<S104>/Integrator' */
 7578       Controller_X.Integrator_CSTATE_g[0] = Controller_P.Integrator_IC_i[0];
 7579       Controller_X.Integrator_CSTATE_g[1] = Controller_P.Integrator_IC_i[1];
 7580       Controller_X.Integrator_CSTATE_g[2] = Controller_P.Integrator_IC_i[2];
 7581   
 7582       /* InitializeConditions for Integrator: '<S106>/Integrator' */
 7583       Controller_X.Integrator_CSTATE_f[0] = Controller_P.Integrator_IC_p;
 7584       Controller_X.Integrator_CSTATE_f[1] = Controller_P.Integrator_IC_p;
 7585       Controller_X.Integrator_CSTATE_f[2] = Controller_P.Integrator_IC_p;
 7586   
 7587       /* InitializeConditions for Integrator: '<S107>/Integrator2' */
 7588       Controller_X.Integrator2_CSTATE[0] = Controller_P.Integrator2_IC[0];
 7589       Controller_X.Integrator2_CSTATE[1] = Controller_P.Integrator2_IC[1];
 7590       Controller_X.Integrator2_CSTATE[2] = Controller_P.Integrator2_IC[2];
 7591       for (i = 0; i < 6; i++) {
 7592         /* InitializeConditions for Integrator: '<S16>/Integrator2' */
 7593         Controller_X.Integrator2_CSTATE_f[i] = Controller_P.Integrator2_IC_h[i];
 7594   
 7595         /* InitializeConditions for Integrator: '<S16>/Integrator6' */
 7596         Controller_X.Integrator6_CSTATE[i] = Controller_P.Integrator6_IC[i];
 7597       }
 7598   
 7599       /* InitializeConditions for TransferFcn: '<S22>/Transfer Fcn' */
 7600       Controller_X.TransferFcn_CSTATE_e = 0.0;
 7601   
 7602       /* InitializeConditions for TransferFcn: '<S22>/Transfer Fcn1' */
 7603       Controller_X.TransferFcn1_CSTATE_d = 0.0;
 7604   
 7605       /* InitializeConditions for TransferFcn: '<S22>/Transfer Fcn2' */
 7606       Controller_X.TransferFcn2_CSTATE_b = 0.0;
 7607   
 7608       /* InitializeConditions for MATLAB Function: '<S16>/reference_model' */
 7609       Controller_DWork.flag[0] = 0.0;
 7610       Controller_DWork.flag[1] = 0.0;
 7611       Controller_DWork.flag[2] = 0.0;
 7612       for (i = 0; i < 18; i++) {
 7613         /* InitializeConditions for Integrator: '<S16>/Integrator10' */
 7614         Controller_X.Integrator10_CSTATE[i] = Controller_P.Integrator10_IC[i];
 7615   
 7616         /* InitializeConditions for Integrator: '<S16>/Integrator3' */
 7617         Controller_X.Integrator3_CSTATE[i] = Controller_P.Integrator3_IC[i];
 7618       }
 7619   
 7620       /* InitializeConditions for MATLAB Function: '<S16>/Control law parameter calculation' */
 7621       Controller_DWork.Lambda_0[0] = 1.0;
 7622       Controller_DWork.Lambda_0[1] = 1.0;
 7623       Controller_DWork.Z_p_bar_hat[0] = 0.0;
 7624       Controller_DWork.Z_p_bar_hat[1] = 0.0;
 7625       Controller_DWork.Z_p_bar_hat[2] = 0.0;
 7626       memset(&Controller_DWork.R_p_hat[0], 0, 9U * sizeof(real_T));
 7627       for (i = 0; i < 6; i++) {
 7628         Controller_DWork.Q[i] = 0.0;
 7629       }
 7630   
 7631       /* End of InitializeConditions for MATLAB Function: '<S16>/Control law parameter calculation' */
 7632   
 7633       /* InitializeConditions for Integrator: '<S16>/Integrator1' */
 7634       for (i = 0; i < 9; i++) {
 7635         Controller_X.Integrator1_CSTATE_m[i] = Controller_P.Integrator1_IC_m[i];
 7636       }
 7637   
 7638       /* End of InitializeConditions for Integrator: '<S16>/Integrator1' */
 7639   
 7640       /* InitializeConditions for Integrator: '<S15>/Integrator' */
 7641       Controller_X.Integrator_CSTATE_p[0] = Controller_P.Integrator_IC_m;
 7642       Controller_X.Integrator_CSTATE_p[1] = Controller_P.Integrator_IC_m;
 7643       Controller_X.Integrator_CSTATE_p[2] = Controller_P.Integrator_IC_m;
 7644   
 7645       /* InitializeConditions for Integrator: '<S33>/Integrator1' */
 7646       Controller_X.Integrator1_CSTATE_f = Controller_P.Integrator1_IC_f;
 7647   
 7648       /* InitializeConditions for Integrator: '<S33>/Integrator' */
 7649       Controller_X.Integrator_CSTATE_c = Controller_P.Integrator_IC_p1;
 7650   
 7651       /* InitializeConditions for MATLAB Function: '<S14>/MATLAB Function' */
 7652       Controller_MATLABFunction_Init();
 7653   
 7654       /* InitializeConditions for Integrator: '<S3>/Integrator' */
 7655       Controller_X.Integrator_CSTATE_j = Controller_P.Integrator_IC_f;
 7656   
 7657       /* InitializeConditions for Integrator: '<S84>/Integrator1' */
 7658       Controller_X.Integrator1_CSTATE_k[0] = Controller_P.Integrator1_IC_a;
 7659       Controller_X.Integrator1_CSTATE_k[1] = Controller_P.Integrator1_IC_a;
 7660       Controller_X.Integrator1_CSTATE_k[2] = Controller_P.Integrator1_IC_a;
 7661   
 7662       /* InitializeConditions for Integrator: '<S84>/Integrator' */
 7663       Controller_X.Integrator_CSTATE_px[0] = Controller_P.Integrator_IC_in;
 7664       Controller_X.Integrator_CSTATE_px[1] = Controller_P.Integrator_IC_in;
 7665       Controller_X.Integrator_CSTATE_px[2] = Controller_P.Integrator_IC_in;
 7666   
 7667       /* InitializeConditions for Integrator: '<S119>/Integrator2' */
 7668       Controller_X.Integrator2_CSTATE_i[0] = Controller_P.Integrator2_IC_a;
 7669       Controller_X.Integrator2_CSTATE_i[1] = Controller_P.Integrator2_IC_a;
 7670       Controller_X.Integrator2_CSTATE_i[2] = Controller_P.Integrator2_IC_a;
 7671   
 7672       /* InitializeConditions for Integrator: '<S93>/Integrator1' */
 7673       if (rtmIsFirstInitCond(Controller_M)) {
 7674         Controller_X.Integrator1_CSTATE_g = 0.0;
 7675       }
 7676   
 7677       Controller_DWork.Integrator1_IWORK.IcNeedsLoading = 1;
 7678   
 7679       /* InitializeConditions for Integrator: '<S93>/Integrator2' */
 7680       if (rtmIsFirstInitCond(Controller_M)) {
 7681         Controller_X.Integrator2_CSTATE_n = 0.0;
 7682       }
 7683   
 7684       Controller_DWork.Integrator2_IWORK.IcNeedsLoading = 1;
 7685   
 7686       /* InitializeConditions for Integrator: '<S93>/Integrator3' */
 7687       if (rtmIsFirstInitCond(Controller_M)) {
 7688         Controller_X.Integrator3_CSTATE_m = 0.0;
 7689       }
 7690   
 7691       Controller_DWork.Integrator3_IWORK.IcNeedsLoading = 1;
 7692   
 7693       /* InitializeConditions for MATLAB Function: '<S82>/MATLAB Function' */
 7694       Controller_DWork.time_reset_x_not_empty = FALSE;
 7695       Controller_DWork.time_reset_y_not_empty = FALSE;
 7696       Controller_DWork.time_reset_psi_not_empty = FALSE;
 7697   
 7698       /* InitializeConditions for MATLAB Function: '<S92>/MATLAB Function' */
 7699       Controller_DWork.nu_x_not_empty = FALSE;
 7700       Controller_DWork.nu_y_not_empty = FALSE;
 7701       Controller_DWork.nu_psi_not_empty = FALSE;
 7702   
 7703       /* InitializeConditions for Integrator: '<S92>/Integrator' */
 7704       Controller_X.Integrator_CSTATE_d[0] = Controller_P.Integrator_IC_e;
 7705       Controller_X.Integrator_CSTATE_d[1] = Controller_P.Integrator_IC_e;
 7706       Controller_X.Integrator_CSTATE_d[2] = Controller_P.Integrator_IC_e;
 7707   
 7708       /* InitializeConditions for MATLAB Function: '<S91>/MATLAB Function' */
 7709       Controller_DWork.d_x_not_empty = FALSE;
 7710       Controller_DWork.d_y_not_empty = FALSE;
 7711       Controller_DWork.d_psi_not_empty = FALSE;
 7712   
 7713       /* InitializeConditions for Integrator: '<S91>/Integrator2' */
 7714       Controller_X.Integrator2_CSTATE_n4[0] = Controller_P.Integrator2_IC_g;
 7715       Controller_X.Integrator2_CSTATE_n4[1] = Controller_P.Integrator2_IC_g;
 7716       Controller_X.Integrator2_CSTATE_n4[2] = Controller_P.Integrator2_IC_g;
 7717   
 7718       /* InitializeConditions for Integrator: '<S16>/Integrator11' */
 7719       Controller_X.Integrator11_CSTATE[0] = Controller_P.Integrator11_IC[0];
 7720       Controller_X.Integrator11_CSTATE[1] = Controller_P.Integrator11_IC[1];
 7721       Controller_X.Integrator11_CSTATE[2] = Controller_P.Integrator11_IC[2];
 7722   
 7723       /* InitializeConditions for Integrator: '<S16>/Integrator4' */
 7724       for (i = 0; i < 30; i++) {
 7725         Controller_X.Integrator4_CSTATE[i] = Controller_P.Integrator4_IC[i];
 7726       }
 7727   
 7728       /* End of InitializeConditions for Integrator: '<S16>/Integrator4' */
 7729   
 7730       /* InitializeConditions for Integrator: '<S16>/Integrator5' */
 7731       Controller_X.Integrator5_CSTATE[0] = Controller_P.Integrator5_IC[0];
 7732       Controller_X.Integrator5_CSTATE[1] = Controller_P.Integrator5_IC[1];
 7733       Controller_X.Integrator5_CSTATE[2] = Controller_P.Integrator5_IC[2];
 7734   
 7735       /* InitializeConditions for Integrator: '<S16>/Integrator9' */
 7736       for (i = 0; i < 18; i++) {
 7737         Controller_X.Integrator9_CSTATE[i] = Controller_P.Integrator9_IC[i];
 7738       }
 7739   
 7740       for (i = 0; i < 6; i++) {
 7741         /* InitializeConditions for MATLAB Function: '<S16>/environment_estimation' */
 7742         Controller_DWork.z1_dot[i] = 0.0;
 7743         Controller_DWork.phi01_dot[i] = 0.0;
 7744         Controller_DWork.phi02_dot[i] = 0.0;
 7745   
 7746         /* InitializeConditions for MATLAB Function: '<S16>/normalized _adaptive_law1' */
 7747         Controller_DWork.dphi01[i] = 0.0;
 7748         Controller_DWork.dphi02[i] = 0.0;
 7749         Controller_DWork.dphi03[i] = 0.0;
 7750         Controller_DWork.dphi04[i] = 0.0;
 7751         Controller_DWork.dphi05[i] = 0.0;
 7752       }
 7753   
 7754       /* End of InitializeConditions for Integrator: '<S16>/Integrator9' */
 7755   
 7756       /* InitializeConditions for MATLAB Function: '<S16>/normalized _adaptive_law1' */
 7757       Controller_DWork.phi1[0] = 0.0;
 7758       Controller_DWork.phi1[1] = 0.0;
 7759       Controller_DWork.phi1[2] = 0.0;
 7760       Controller_DWork.phi2[0] = 0.0;
 7761       Controller_DWork.phi2[1] = 0.0;
 7762       Controller_DWork.phi2[2] = 0.0;
 7763       Controller_DWork.phi3[0] = 0.0;
 7764       Controller_DWork.phi3[1] = 0.0;
 7765       Controller_DWork.phi3[2] = 0.0;
 7766       Controller_DWork.phi5[0] = 0.0;
 7767       Controller_DWork.phi5[1] = 0.0;
 7768       Controller_DWork.phi5[2] = 0.0;
 7769   
 7770       /* InitializeConditions for TransferFcn: '<S59>/Transfer Fcn' */
 7771       Controller_X.TransferFcn_CSTATE_en[0] = 0.0;
 7772       Controller_X.TransferFcn_CSTATE_en[1] = 0.0;
 7773   
 7774       /* InitializeConditions for TransferFcn: '<S59>/Transfer Fcn1' */
 7775       Controller_X.TransferFcn1_CSTATE_m[0] = 0.0;
 7776       Controller_X.TransferFcn1_CSTATE_m[1] = 0.0;
 7777   
 7778       /* InitializeConditions for TransferFcn: '<S59>/Transfer Fcn2' */
 7779       Controller_X.TransferFcn2_CSTATE_j[0] = 0.0;
 7780       Controller_X.TransferFcn2_CSTATE_j[1] = 0.0;
 7781   
 7782       /* InitializeConditions for TransferFcn: '<S64>/Transfer Fcn' */
 7783       Controller_X.TransferFcn_CSTATE_d[0] = 0.0;
 7784       Controller_X.TransferFcn_CSTATE_d[1] = 0.0;
 7785   
 7786       /* InitializeConditions for TransferFcn: '<S64>/Transfer Fcn1' */
 7787       Controller_X.TransferFcn1_CSTATE_e[0] = 0.0;
 7788       Controller_X.TransferFcn1_CSTATE_e[1] = 0.0;
 7789   
 7790       /* InitializeConditions for TransferFcn: '<S64>/Transfer Fcn2' */
 7791       Controller_X.TransferFcn2_CSTATE_c[0] = 0.0;
 7792       Controller_X.TransferFcn2_CSTATE_c[1] = 0.0;
 7793   
 7794       /* InitializeConditions for TransferFcn: '<S69>/Transfer Fcn' */
 7795       Controller_X.TransferFcn_CSTATE_o[0] = 0.0;
 7796       Controller_X.TransferFcn_CSTATE_o[1] = 0.0;
 7797   
 7798       /* InitializeConditions for TransferFcn: '<S69>/Transfer Fcn1' */
 7799       Controller_X.TransferFcn1_CSTATE_g[0] = 0.0;
 7800       Controller_X.TransferFcn1_CSTATE_g[1] = 0.0;
 7801   
 7802       /* InitializeConditions for TransferFcn: '<S69>/Transfer Fcn2' */
 7803       Controller_X.TransferFcn2_CSTATE_i[0] = 0.0;
 7804       Controller_X.TransferFcn2_CSTATE_i[1] = 0.0;
 7805   
 7806       /* InitializeConditions for TransferFcn: '<S74>/Transfer Fcn' */
 7807       Controller_X.TransferFcn_CSTATE_c[0] = 0.0;
 7808       Controller_X.TransferFcn_CSTATE_c[1] = 0.0;
 7809   
 7810       /* InitializeConditions for TransferFcn: '<S74>/Transfer Fcn1' */
 7811       Controller_X.TransferFcn1_CSTATE_l[0] = 0.0;
 7812       Controller_X.TransferFcn1_CSTATE_l[1] = 0.0;
 7813   
 7814       /* InitializeConditions for TransferFcn: '<S74>/Transfer Fcn2' */
 7815       Controller_X.TransferFcn2_CSTATE_h[0] = 0.0;
 7816       Controller_X.TransferFcn2_CSTATE_h[1] = 0.0;
 7817   
 7818       /* InitializeConditions for TransferFcn: '<S76>/Transfer Fcn' */
 7819       Controller_X.TransferFcn_CSTATE_n[0] = 0.0;
 7820       Controller_X.TransferFcn_CSTATE_n[1] = 0.0;
 7821   
 7822       /* InitializeConditions for TransferFcn: '<S76>/Transfer Fcn1' */
 7823       Controller_X.TransferFcn1_CSTATE_p[0] = 0.0;
 7824       Controller_X.TransferFcn1_CSTATE_p[1] = 0.0;
 7825   
 7826       /* InitializeConditions for TransferFcn: '<S76>/Transfer Fcn2' */
 7827       Controller_X.TransferFcn2_CSTATE_ck[0] = 0.0;
 7828       Controller_X.TransferFcn2_CSTATE_ck[1] = 0.0;
 7829   
 7830       /* InitializeConditions for Integrator: '<S108>/Integrator' */
 7831       Controller_X.Integrator_CSTATE_l[0] = Controller_P.Integrator_IC_ik[0];
 7832       Controller_X.Integrator_CSTATE_l[1] = Controller_P.Integrator_IC_ik[1];
 7833       Controller_X.Integrator_CSTATE_l[2] = Controller_P.Integrator_IC_ik[2];
 7834   
 7835       /* InitializeConditions for Integrator: '<S106>/Integrator1' */
 7836       Controller_X.Integrator1_CSTATE_b[0] = Controller_P.Integrator1_IC_c;
 7837       Controller_X.Integrator1_CSTATE_b[1] = Controller_P.Integrator1_IC_c;
 7838       Controller_X.Integrator1_CSTATE_b[2] = Controller_P.Integrator1_IC_c;
 7839   
 7840       /* InitializeConditions for Integrator: '<S106>/Integrator2' */
 7841       Controller_X.Integrator2_CSTATE_h[0] = Controller_P.Integrator2_IC_hf;
 7842       Controller_X.Integrator2_CSTATE_h[1] = Controller_P.Integrator2_IC_hf;
 7843       Controller_X.Integrator2_CSTATE_h[2] = Controller_P.Integrator2_IC_hf;
 7844   
 7845       /* set "at time zero" to false */
 7846       if (rtmIsFirstInitCond(Controller_M)) {
 7847         rtmSetFirstInitCond(Controller_M, 0);
 7848       }
 7849     }
 7850   }
 7851   
 7852   /* Model terminate function */
 7853   void Controller_terminate(void)
 7854   {
 7855   }
 7856