File: Controller.cpp1 /* 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 |