File: bh_f14.c1 /* 2 * File: bh_f14.c 3 * 4 * Code generated for Simulink model 'bh_f14'. 5 * 6 * Model version : 1.18 7 * Simulink Coder version : 8.10 (R2016a) 10-Feb-2016 8 * C/C++ source code generated on : Wed Jun 22 07:56:08 2016 9 * 10 * Target selection: ert.tlc 11 * Embedded hardware selection: 32-bit Generic 12 * Code generation objectives: Unspecified 13 * Validation result: Not run 14 */ 15 16 #include "bh_f14.h" 17 #include "bh_f14_private.h" 18 19 static void rate_scheduler(RT_MODEL_bh_f14_T *const bh_f14_M); 20 21 /* 22 * This function updates active task flag for each subrate. 23 * The function is called at model base rate, hence the 24 * generated code self-manages all its subrates. 25 */ 26 static void rate_scheduler(RT_MODEL_bh_f14_T *const bh_f14_M) 27 { 28 /* Compute which subrates run during the next base time step. Subrates 29 * are an integer multiple of the base rate counter. Therefore, the subtask 30 * counter is reset when it reaches its limit (zero means run). 31 */ 32 (bh_f14_M->Timing.TaskCounters.TID[2])++; 33 if ((bh_f14_M->Timing.TaskCounters.TID[2]) > 9) {/* Sample time: [0.1s, 0.0s] */ 34 bh_f14_M->Timing.TaskCounters.TID[2] = 0; 35 } 36 } 37 38 /* 39 * This function updates continuous states using the ODE4 fixed-step 40 * solver algorithm 41 */ 42 static void rt_ertODEUpdateContinuousStates(RTWSolverInfo *si , 43 RT_MODEL_bh_f14_T *const bh_f14_M, real_T bh_f14_U_u, real_T 44 *bh_f14_Y_alpharad, real_T *bh_f14_Y_NzPilotg) 45 { 46 time_T t = rtsiGetT(si); 47 time_T tnew = rtsiGetSolverStopTime(si); 48 time_T h = rtsiGetStepSize(si); 49 real_T *x = rtsiGetContStates(si); 50 ODE4_IntgData *id = (ODE4_IntgData *)rtsiGetSolverData(si); 51 real_T *y = id->y; 52 real_T *f0 = id->f[0]; 53 real_T *f1 = id->f[1]; 54 real_T *f2 = id->f[2]; 55 real_T *f3 = id->f[3]; 56 real_T temp; 57 int_T i; 58 int_T nXc = 10; 59 rtsiSetSimTimeStep(si,MINOR_TIME_STEP); 60 61 /* Save the state values at time t in y, we'll use x as ynew. */ 62 (void) memcpy(y, x, 63 (uint_T)nXc*sizeof(real_T)); 64 65 /* Assumes that rtsiSetT and ModelOutputs are up-to-date */ 66 /* f0 = f(t,y) */ 67 rtsiSetdX(si, f0); 68 bh_f14_derivatives(bh_f14_M); 69 70 /* f1 = f(t + (h/2), y + (h/2)*f0) */ 71 temp = 0.5 * h; 72 for (i = 0; i < nXc; i++) { 73 x[i] = y[i] + (temp*f0[i]); 74 } 75 76 rtsiSetT(si, t + temp); 77 rtsiSetdX(si, f1); 78 bh_f14_step(bh_f14_M, bh_f14_U_u, bh_f14_Y_alpharad, bh_f14_Y_NzPilotg); 79 bh_f14_derivatives(bh_f14_M); 80 81 /* f2 = f(t + (h/2), y + (h/2)*f1) */ 82 for (i = 0; i < nXc; i++) { 83 x[i] = y[i] + (temp*f1[i]); 84 } 85 86 rtsiSetdX(si, f2); 87 bh_f14_step(bh_f14_M, bh_f14_U_u, bh_f14_Y_alpharad, bh_f14_Y_NzPilotg); 88 bh_f14_derivatives(bh_f14_M); 89 90 /* f3 = f(t + h, y + h*f2) */ 91 for (i = 0; i < nXc; i++) { 92 x[i] = y[i] + (h*f2[i]); 93 } 94 95 rtsiSetT(si, tnew); 96 rtsiSetdX(si, f3); 97 bh_f14_step(bh_f14_M, bh_f14_U_u, bh_f14_Y_alpharad, bh_f14_Y_NzPilotg); 98 bh_f14_derivatives(bh_f14_M); 99 100 /* tnew = t + h 101 ynew = y + (h/6)*(f0 + 2*f1 + 2*f2 + 2*f3) */ 102 temp = h / 6.0; 103 for (i = 0; i < nXc; i++) { 104 x[i] = y[i] + temp*(f0[i] + 2.0*f1[i] + 2.0*f2[i] + f3[i]); 105 } 106 107 rtsiSetSimTimeStep(si,MAJOR_TIME_STEP); 108 } 109 110 real_T rt_urand_Upu32_Yd_f_pw_snf(uint32_T *u) 111 { 112 uint32_T lo; 113 uint32_T hi; 114 115 /* Uniform random number generator (random number between 0 and 1) 116 117 #define IA 16807 magic multiplier = 7^5 118 #define IM 2147483647 modulus = 2^31-1 119 #define IQ 127773 IM div IA 120 #define IR 2836 IM modulo IA 121 #define S 4.656612875245797e-10 reciprocal of 2^31-1 122 test = IA * (seed % IQ) - IR * (seed/IQ) 123 seed = test < 0 ? (test + IM) : test 124 return (seed*S) 125 */ 126 lo = *u % 127773U * 16807U; 127 hi = *u / 127773U * 2836U; 128 if (lo < hi) { 129 *u = 2147483647U - (hi - lo); 130 } else { 131 *u = lo - hi; 132 } 133 134 return (real_T)*u * 4.6566128752457969E-10; 135 } 136 137 real_T rt_nrand_Upu32_Yd_f_pw_snf(uint32_T *u) 138 { 139 real_T y; 140 real_T sr; 141 real_T si; 142 143 /* Normal (Gaussian) random number generator */ 144 do { 145 sr = 2.0 * rt_urand_Upu32_Yd_f_pw_snf(u) - 1.0; 146 si = 2.0 * rt_urand_Upu32_Yd_f_pw_snf(u) - 1.0; 147 si = sr * sr + si * si; 148 } while (si > 1.0); 149 150 y = sqrt(-2.0 * log(si) / si) * sr; 151 return y; 152 } 153 154 /* Model step function */ 155 void bh_f14_step(RT_MODEL_bh_f14_T *const bh_f14_M, real_T bh_f14_U_u, real_T 156 *bh_f14_Y_alpharad, real_T *bh_f14_Y_NzPilotg) 157 { 158 B_bh_f14_T *bh_f14_B = ((B_bh_f14_T *) bh_f14_M->ModelData.blockIO); 159 DW_bh_f14_T *bh_f14_DW = ((DW_bh_f14_T *) bh_f14_M->ModelData.dwork); 160 X_bh_f14_T *bh_f14_X = ((X_bh_f14_T *) bh_f14_M->ModelData.contStates); 161 if (rtmIsMajorTimeStep(bh_f14_M)) { 162 /* set solver stop time */ 163 rtsiSetSolverStopTime(&bh_f14_M->solverInfo,((bh_f14_M->Timing.clockTick0+1)* 164 bh_f14_M->Timing.stepSize0)); 165 } /* end MajorTimeStep */ 166 167 /* Update absolute time of base rate at minor time step */ 168 if (rtmIsMinorTimeStep(bh_f14_M)) { 169 bh_f14_M->Timing.t[0] = rtsiGetT(&bh_f14_M->solverInfo); 170 } 171 172 { 173 real_T *lastU; 174 real_T rtb_Actuator; 175 real_T rtb_AlphasensorLowpassFilter; 176 177 /* TransferFcn: '<S3>/Transfer Fcn.2' */ 178 bh_f14_B->TransferFcn2 = 0.0; 179 bh_f14_B->TransferFcn2 += 1.0 * bh_f14_X->TransferFcn2_CSTATE; 180 181 /* Gain: '<Root>/Gain5' */ 182 bh_f14_B->Gain5 = 0.001450536698578474 * bh_f14_B->TransferFcn2; 183 184 /* Outport: '<Root>/alpha (rad)' */ 185 *bh_f14_Y_alpharad = bh_f14_B->Gain5; 186 187 /* Derivative: '<S2>/Derivative' */ 188 if ((bh_f14_DW->TimeStampA >= bh_f14_M->Timing.t[0]) && 189 (bh_f14_DW->TimeStampB >= bh_f14_M->Timing.t[0])) { 190 rtb_AlphasensorLowpassFilter = 0.0; 191 } else { 192 rtb_Actuator = bh_f14_DW->TimeStampA; 193 lastU = &bh_f14_DW->LastUAtTimeA; 194 if (bh_f14_DW->TimeStampA < bh_f14_DW->TimeStampB) { 195 if (bh_f14_DW->TimeStampB < bh_f14_M->Timing.t[0]) { 196 rtb_Actuator = bh_f14_DW->TimeStampB; 197 lastU = &bh_f14_DW->LastUAtTimeB; 198 } 199 } else { 200 if (bh_f14_DW->TimeStampA >= bh_f14_M->Timing.t[0]) { 201 rtb_Actuator = bh_f14_DW->TimeStampB; 202 lastU = &bh_f14_DW->LastUAtTimeB; 203 } 204 } 205 206 rtb_AlphasensorLowpassFilter = (bh_f14_B->TransferFcn2 - *lastU) / 207 (bh_f14_M->Timing.t[0] - rtb_Actuator); 208 } 209 210 /* End of Derivative: '<S2>/Derivative' */ 211 212 /* TransferFcn: '<S3>/Transfer Fcn.1' */ 213 bh_f14_B->TransferFcn1 = 0.0; 214 bh_f14_B->TransferFcn1 += 1.0 * bh_f14_X->TransferFcn1_CSTATE; 215 216 /* Derivative: '<S2>/Derivative1' */ 217 if ((bh_f14_DW->TimeStampA_o >= bh_f14_M->Timing.t[0]) && 218 (bh_f14_DW->TimeStampB_h >= bh_f14_M->Timing.t[0])) { 219 rtb_Actuator = 0.0; 220 } else { 221 rtb_Actuator = bh_f14_DW->TimeStampA_o; 222 lastU = &bh_f14_DW->LastUAtTimeA_i; 223 if (bh_f14_DW->TimeStampA_o < bh_f14_DW->TimeStampB_h) { 224 if (bh_f14_DW->TimeStampB_h < bh_f14_M->Timing.t[0]) { 225 rtb_Actuator = bh_f14_DW->TimeStampB_h; 226 lastU = &bh_f14_DW->LastUAtTimeB_j; 227 } 228 } else { 229 if (bh_f14_DW->TimeStampA_o >= bh_f14_M->Timing.t[0]) { 230 rtb_Actuator = bh_f14_DW->TimeStampB_h; 231 lastU = &bh_f14_DW->LastUAtTimeB_j; 232 } 233 } 234 235 rtb_Actuator = (bh_f14_B->TransferFcn1 - *lastU) / (bh_f14_M->Timing.t[0] 236 - rtb_Actuator); 237 } 238 239 /* End of Derivative: '<S2>/Derivative1' */ 240 241 /* Gain: '<S2>/Gain2' incorporates: 242 * Constant: '<S2>/Constant' 243 * Gain: '<S2>/Gain1' 244 * Product: '<S2>/Product' 245 * Sum: '<S2>/Sum1' 246 */ 247 bh_f14_B->Gain2 = ((22.8 * rtb_Actuator - rtb_AlphasensorLowpassFilter) + 248 bh_f14_B->TransferFcn1 * 689.4) * 0.031055900621118009; 249 250 /* Outport: '<Root>/Nz Pilot (g)' */ 251 *bh_f14_Y_NzPilotg = bh_f14_B->Gain2; 252 if (rtmIsMajorTimeStep(bh_f14_M) && 253 bh_f14_M->Timing.TaskCounters.TID[1] == 0) { 254 } 255 256 /* SignalGenerator: '<Root>/Pilot' */ 257 rtb_Actuator = 0.079577471545947673 * bh_f14_M->Timing.t[0]; 258 if (rtb_Actuator - floor(rtb_Actuator) >= 0.5) { 259 rtb_Actuator = 1.0; 260 } else { 261 rtb_Actuator = -1.0; 262 } 263 264 /* End of SignalGenerator: '<Root>/Pilot' */ 265 266 /* Sum: '<Root>/Sum1' incorporates: 267 * Inport: '<Root>/u' 268 */ 269 bh_f14_B->Sum1 = rtb_Actuator + bh_f14_U_u; 270 if (rtmIsMajorTimeStep(bh_f14_M) && 271 bh_f14_M->Timing.TaskCounters.TID[1] == 0) { 272 } 273 274 /* TransferFcn: '<Root>/Actuator' */ 275 rtb_Actuator = 20.0 * bh_f14_X->Actuator_CSTATE; 276 277 /* Sum: '<S1>/Sum2' incorporates: 278 * Gain: '<S1>/Gain2' 279 * Gain: '<S1>/Gain3' 280 * Sum: '<S1>/Sum1' 281 * TransferFcn: '<S1>/Alpha-sensor Low-pass Filter' 282 * TransferFcn: '<S1>/Pitch Rate Lead Filter' 283 * TransferFcn: '<S1>/Stick Prefilter' 284 */ 285 bh_f14_B->Sum2 = 10.0 * bh_f14_X->StickPrefilter_CSTATE - (((-1.173) * 286 bh_f14_X->PitchRateLeadFilter_CSTATE + 1.0 * bh_f14_B->TransferFcn1) * 287 0.8156 + 2.5258903763576663 * bh_f14_X->AlphasensorLowpassFilter_CSTATE * 288 0.677); 289 290 /* Sum: '<S1>/Sum' incorporates: 291 * Gain: '<S1>/Gain' 292 * TransferFcn: '<S1>/Proportional plus integral compensator' 293 */ 294 bh_f14_B->Sum = (-1.746) * bh_f14_B->Sum2 + (-3.864) * 295 bh_f14_X->Proportionalplusintegralcompens; 296 297 /* TransferFcn: '<S4>/W-gust model' */ 298 bh_f14_B->Wgustmodel = 0.0; 299 bh_f14_B->Wgustmodel += 3.2636985073652349 * bh_f14_X->Wgustmodel_CSTATE[0]; 300 bh_f14_B->Wgustmodel += 0.74337115819042854 * bh_f14_X->Wgustmodel_CSTATE[1]; 301 302 /* Sum: '<S3>/Sum1' incorporates: 303 * Gain: '<Root>/Gain' 304 * Gain: '<S3>/Gain3' 305 * Gain: '<S3>/Gain5' 306 */ 307 bh_f14_B->Sum1_o = ((-63.9979) * rtb_Actuator - (-0.6385) * 308 bh_f14_B->Wgustmodel) + 689.4 * bh_f14_B->TransferFcn1; 309 310 /* Sum: '<S3>/Sum2' incorporates: 311 * Gain: '<Root>/Gain1' 312 * Gain: '<Root>/Gain2' 313 * Gain: '<S3>/Gain4' 314 * Gain: '<S3>/Gain6' 315 * Sum: '<Root>/Sum' 316 * TransferFcn: '<S4>/Q-gust model' 317 */ 318 bh_f14_B->Sum2_h = ((-0.00592) * bh_f14_B->TransferFcn2 - 319 (((-0.10355189850799512) * bh_f14_X->Qgustmodel_CSTATE + 320 0.01224696964599171 * bh_f14_B->Wgustmodel) * (-0.6571) 321 + (-0.00592) * bh_f14_B->Wgustmodel)) + (-6.8847) * 322 rtb_Actuator; 323 if (rtmIsMajorTimeStep(bh_f14_M) && 324 bh_f14_M->Timing.TaskCounters.TID[2] == 0) { 325 /* Gain: '<S5>/Output' incorporates: 326 * RandomNumber: '<S5>/White Noise' 327 */ 328 bh_f14_B->Output = 3.1622776601683791 * bh_f14_DW->NextOutput; 329 } 330 } 331 332 if (rtmIsMajorTimeStep(bh_f14_M)) { 333 real_T *lastU; 334 335 /* Update for Derivative: '<S2>/Derivative' */ 336 if (bh_f14_DW->TimeStampA == (rtInf)) { 337 bh_f14_DW->TimeStampA = bh_f14_M->Timing.t[0]; 338 lastU = &bh_f14_DW->LastUAtTimeA; 339 } else if (bh_f14_DW->TimeStampB == (rtInf)) { 340 bh_f14_DW->TimeStampB = bh_f14_M->Timing.t[0]; 341 lastU = &bh_f14_DW->LastUAtTimeB; 342 } else if (bh_f14_DW->TimeStampA < bh_f14_DW->TimeStampB) { 343 bh_f14_DW->TimeStampA = bh_f14_M->Timing.t[0]; 344 lastU = &bh_f14_DW->LastUAtTimeA; 345 } else { 346 bh_f14_DW->TimeStampB = bh_f14_M->Timing.t[0]; 347 lastU = &bh_f14_DW->LastUAtTimeB; 348 } 349 350 *lastU = bh_f14_B->TransferFcn2; 351 352 /* End of Update for Derivative: '<S2>/Derivative' */ 353 354 /* Update for Derivative: '<S2>/Derivative1' */ 355 if (bh_f14_DW->TimeStampA_o == (rtInf)) { 356 bh_f14_DW->TimeStampA_o = bh_f14_M->Timing.t[0]; 357 lastU = &bh_f14_DW->LastUAtTimeA_i; 358 } else if (bh_f14_DW->TimeStampB_h == (rtInf)) { 359 bh_f14_DW->TimeStampB_h = bh_f14_M->Timing.t[0]; 360 lastU = &bh_f14_DW->LastUAtTimeB_j; 361 } else if (bh_f14_DW->TimeStampA_o < bh_f14_DW->TimeStampB_h) { 362 bh_f14_DW->TimeStampA_o = bh_f14_M->Timing.t[0]; 363 lastU = &bh_f14_DW->LastUAtTimeA_i; 364 } else { 365 bh_f14_DW->TimeStampB_h = bh_f14_M->Timing.t[0]; 366 lastU = &bh_f14_DW->LastUAtTimeB_j; 367 } 368 369 *lastU = bh_f14_B->TransferFcn1; 370 371 /* End of Update for Derivative: '<S2>/Derivative1' */ 372 if (rtmIsMajorTimeStep(bh_f14_M) && 373 bh_f14_M->Timing.TaskCounters.TID[2] == 0) { 374 /* Update for RandomNumber: '<S5>/White Noise' */ 375 bh_f14_DW->NextOutput = rt_nrand_Upu32_Yd_f_pw_snf(&bh_f14_DW->RandSeed) * 376 1.0 + 0.0; 377 } 378 } /* end MajorTimeStep */ 379 380 if (rtmIsMajorTimeStep(bh_f14_M)) { 381 rt_ertODEUpdateContinuousStates(&bh_f14_M->solverInfo, bh_f14_M, bh_f14_U_u, 382 bh_f14_Y_alpharad, bh_f14_Y_NzPilotg); 383 384 /* Update absolute time for base rate */ 385 /* The "clockTick0" counts the number of times the code of this task has 386 * been executed. The absolute time is the multiplication of "clockTick0" 387 * and "Timing.stepSize0". Size of "clockTick0" ensures timer will not 388 * overflow during the application lifespan selected. 389 */ 390 ++bh_f14_M->Timing.clockTick0; 391 bh_f14_M->Timing.t[0] = rtsiGetSolverStopTime(&bh_f14_M->solverInfo); 392 393 { 394 /* Update absolute timer for sample time: [0.01s, 0.0s] */ 395 /* The "clockTick1" counts the number of times the code of this task has 396 * been executed. The resolution of this integer timer is 0.01, which is the step size 397 * of the task. Size of "clockTick1" ensures timer will not overflow during the 398 * application lifespan selected. 399 */ 400 bh_f14_M->Timing.clockTick1++; 401 } 402 403 rate_scheduler(bh_f14_M); 404 } /* end MajorTimeStep */ 405 } 406 407 /* Derivatives for root system: '<Root>' */ 408 void bh_f14_derivatives(RT_MODEL_bh_f14_T *const bh_f14_M) 409 { 410 B_bh_f14_T *bh_f14_B = ((B_bh_f14_T *) bh_f14_M->ModelData.blockIO); 411 X_bh_f14_T *bh_f14_X = ((X_bh_f14_T *) bh_f14_M->ModelData.contStates); 412 XDot_bh_f14_T *_rtXdot; 413 _rtXdot = ((XDot_bh_f14_T *) bh_f14_M->ModelData.derivs); 414 415 /* Derivatives for TransferFcn: '<S3>/Transfer Fcn.2' */ 416 _rtXdot->TransferFcn2_CSTATE = 0.0; 417 _rtXdot->TransferFcn2_CSTATE += (-0.6385) * bh_f14_X->TransferFcn2_CSTATE; 418 _rtXdot->TransferFcn2_CSTATE += bh_f14_B->Sum1_o; 419 420 /* Derivatives for TransferFcn: '<S3>/Transfer Fcn.1' */ 421 _rtXdot->TransferFcn1_CSTATE = 0.0; 422 _rtXdot->TransferFcn1_CSTATE += (-0.6571) * bh_f14_X->TransferFcn1_CSTATE; 423 _rtXdot->TransferFcn1_CSTATE += bh_f14_B->Sum2_h; 424 425 /* Derivatives for TransferFcn: '<Root>/Actuator' */ 426 _rtXdot->Actuator_CSTATE = 0.0; 427 _rtXdot->Actuator_CSTATE += (-20.0) * bh_f14_X->Actuator_CSTATE; 428 _rtXdot->Actuator_CSTATE += bh_f14_B->Sum; 429 430 /* Derivatives for TransferFcn: '<S1>/Alpha-sensor Low-pass Filter' */ 431 _rtXdot->AlphasensorLowpassFilter_CSTATE = 0.0; 432 _rtXdot->AlphasensorLowpassFilter_CSTATE += (-2.5258903763576663) * 433 bh_f14_X->AlphasensorLowpassFilter_CSTATE; 434 _rtXdot->AlphasensorLowpassFilter_CSTATE += bh_f14_B->Gain5; 435 436 /* Derivatives for TransferFcn: '<S1>/Stick Prefilter' */ 437 _rtXdot->StickPrefilter_CSTATE = 0.0; 438 _rtXdot->StickPrefilter_CSTATE += (-10.0) * bh_f14_X->StickPrefilter_CSTATE; 439 _rtXdot->StickPrefilter_CSTATE += bh_f14_B->Sum1; 440 441 /* Derivatives for TransferFcn: '<S1>/Pitch Rate Lead Filter' */ 442 _rtXdot->PitchRateLeadFilter_CSTATE = 0.0; 443 _rtXdot->PitchRateLeadFilter_CSTATE += (-4.144) * 444 bh_f14_X->PitchRateLeadFilter_CSTATE; 445 _rtXdot->PitchRateLeadFilter_CSTATE += bh_f14_B->TransferFcn1; 446 447 /* Derivatives for TransferFcn: '<S1>/Proportional plus integral compensator' */ 448 _rtXdot->Proportionalplusintegralcompens = 0.0; 449 _rtXdot->Proportionalplusintegralcompens += (-0.0) * 450 bh_f14_X->Proportionalplusintegralcompens; 451 _rtXdot->Proportionalplusintegralcompens += bh_f14_B->Sum2; 452 453 /* Derivatives for TransferFcn: '<S4>/W-gust model' */ 454 _rtXdot->Wgustmodel_CSTATE[0] = 0.0; 455 _rtXdot->Wgustmodel_CSTATE[0] += (-0.78901688496133815) * 456 bh_f14_X->Wgustmodel_CSTATE[0]; 457 _rtXdot->Wgustmodel_CSTATE[1] = 0.0; 458 _rtXdot->Wgustmodel_CSTATE[0] += (-0.15563691118852338) * 459 bh_f14_X->Wgustmodel_CSTATE[1]; 460 _rtXdot->Wgustmodel_CSTATE[1] += bh_f14_X->Wgustmodel_CSTATE[0]; 461 _rtXdot->Wgustmodel_CSTATE[0] += bh_f14_B->Output; 462 463 /* Derivatives for TransferFcn: '<S4>/Q-gust model' */ 464 _rtXdot->Qgustmodel_CSTATE = 0.0; 465 _rtXdot->Qgustmodel_CSTATE += (-8.4553078435926761) * 466 bh_f14_X->Qgustmodel_CSTATE; 467 _rtXdot->Qgustmodel_CSTATE += bh_f14_B->Wgustmodel; 468 } 469 470 /* Model initialize function */ 471 void bh_f14_initialize(RT_MODEL_bh_f14_T *const bh_f14_M, real_T *bh_f14_U_u, 472 real_T *bh_f14_Y_alpharad, real_T *bh_f14_Y_NzPilotg) 473 { 474 DW_bh_f14_T *bh_f14_DW = ((DW_bh_f14_T *) bh_f14_M->ModelData.dwork); 475 X_bh_f14_T *bh_f14_X = ((X_bh_f14_T *) bh_f14_M->ModelData.contStates); 476 B_bh_f14_T *bh_f14_B = ((B_bh_f14_T *) bh_f14_M->ModelData.blockIO); 477 478 /* Registration code */ 479 480 /* initialize non-finites */ 481 rt_InitInfAndNaN(sizeof(real_T)); 482 483 { 484 /* Setup solver object */ 485 rtsiSetSimTimeStepPtr(&bh_f14_M->solverInfo, &bh_f14_M->Timing.simTimeStep); 486 rtsiSetTPtr(&bh_f14_M->solverInfo, &rtmGetTPtr(bh_f14_M)); 487 rtsiSetStepSizePtr(&bh_f14_M->solverInfo, &bh_f14_M->Timing.stepSize0); 488 rtsiSetdXPtr(&bh_f14_M->solverInfo, &bh_f14_M->ModelData.derivs); 489 rtsiSetContStatesPtr(&bh_f14_M->solverInfo, (real_T **) 490 &bh_f14_M->ModelData.contStates); 491 rtsiSetNumContStatesPtr(&bh_f14_M->solverInfo, 492 &bh_f14_M->Sizes.numContStates); 493 rtsiSetNumPeriodicContStatesPtr(&bh_f14_M->solverInfo, 494 &bh_f14_M->Sizes.numPeriodicContStates); 495 rtsiSetPeriodicContStateIndicesPtr(&bh_f14_M->solverInfo, 496 &bh_f14_M->ModelData.periodicContStateIndices); 497 rtsiSetPeriodicContStateRangesPtr(&bh_f14_M->solverInfo, 498 &bh_f14_M->ModelData.periodicContStateRanges); 499 rtsiSetErrorStatusPtr(&bh_f14_M->solverInfo, (&rtmGetErrorStatus(bh_f14_M))); 500 rtsiSetRTModelPtr(&bh_f14_M->solverInfo, bh_f14_M); 501 } 502 503 rtsiSetSimTimeStep(&bh_f14_M->solverInfo, MAJOR_TIME_STEP); 504 bh_f14_M->ModelData.intgData.y = bh_f14_M->ModelData.odeY; 505 bh_f14_M->ModelData.intgData.f[0] = bh_f14_M->ModelData.odeF[0]; 506 bh_f14_M->ModelData.intgData.f[1] = bh_f14_M->ModelData.odeF[1]; 507 bh_f14_M->ModelData.intgData.f[2] = bh_f14_M->ModelData.odeF[2]; 508 bh_f14_M->ModelData.intgData.f[3] = bh_f14_M->ModelData.odeF[3]; 509 bh_f14_M->ModelData.contStates = ((X_bh_f14_T *) bh_f14_X); 510 rtsiSetSolverData(&bh_f14_M->solverInfo, (void *)&bh_f14_M->ModelData.intgData); 511 rtsiSetSolverName(&bh_f14_M->solverInfo,"ode4"); 512 rtmSetTPtr(bh_f14_M, &bh_f14_M->Timing.tArray[0]); 513 bh_f14_M->Timing.stepSize0 = 0.01; 514 515 /* block I/O */ 516 { 517 bh_f14_B->TransferFcn2 = 0.0; 518 bh_f14_B->Gain5 = 0.0; 519 bh_f14_B->TransferFcn1 = 0.0; 520 bh_f14_B->Gain2 = 0.0; 521 bh_f14_B->Sum1 = 0.0; 522 bh_f14_B->Sum2 = 0.0; 523 bh_f14_B->Sum = 0.0; 524 bh_f14_B->Wgustmodel = 0.0; 525 bh_f14_B->Sum1_o = 0.0; 526 bh_f14_B->Sum2_h = 0.0; 527 bh_f14_B->Output = 0.0; 528 } 529 530 /* states (continuous) */ 531 { 532 (void) memset((void *)bh_f14_X, 0, 533 sizeof(X_bh_f14_T)); 534 } 535 536 /* states (dwork) */ 537 (void) memset((void *)bh_f14_DW, 0, 538 sizeof(DW_bh_f14_T)); 539 bh_f14_DW->TimeStampA = 0.0; 540 bh_f14_DW->LastUAtTimeA = 0.0; 541 bh_f14_DW->TimeStampB = 0.0; 542 bh_f14_DW->LastUAtTimeB = 0.0; 543 bh_f14_DW->TimeStampA_o = 0.0; 544 bh_f14_DW->LastUAtTimeA_i = 0.0; 545 bh_f14_DW->TimeStampB_h = 0.0; 546 bh_f14_DW->LastUAtTimeB_j = 0.0; 547 bh_f14_DW->NextOutput = 0.0; 548 549 /* external inputs */ 550 (*bh_f14_U_u) = 0.0; 551 552 /* external outputs */ 553 (*bh_f14_Y_alpharad) = 0.0; 554 (*bh_f14_Y_NzPilotg) = 0.0; 555 556 { 557 uint32_T tseed; 558 int32_T r; 559 int32_T t; 560 real_T tmp; 561 562 /* InitializeConditions for TransferFcn: '<S3>/Transfer Fcn.2' */ 563 bh_f14_X->TransferFcn2_CSTATE = 0.0; 564 565 /* InitializeConditions for Derivative: '<S2>/Derivative' */ 566 bh_f14_DW->TimeStampA = (rtInf); 567 bh_f14_DW->TimeStampB = (rtInf); 568 569 /* InitializeConditions for TransferFcn: '<S3>/Transfer Fcn.1' */ 570 bh_f14_X->TransferFcn1_CSTATE = 0.0; 571 572 /* InitializeConditions for Derivative: '<S2>/Derivative1' */ 573 bh_f14_DW->TimeStampA_o = (rtInf); 574 bh_f14_DW->TimeStampB_h = (rtInf); 575 576 /* InitializeConditions for TransferFcn: '<Root>/Actuator' */ 577 bh_f14_X->Actuator_CSTATE = 0.0; 578 579 /* InitializeConditions for TransferFcn: '<S1>/Alpha-sensor Low-pass Filter' */ 580 bh_f14_X->AlphasensorLowpassFilter_CSTATE = 0.0; 581 582 /* InitializeConditions for TransferFcn: '<S1>/Stick Prefilter' */ 583 bh_f14_X->StickPrefilter_CSTATE = 0.0; 584 585 /* InitializeConditions for TransferFcn: '<S1>/Pitch Rate Lead Filter' */ 586 bh_f14_X->PitchRateLeadFilter_CSTATE = 0.0; 587 588 /* InitializeConditions for TransferFcn: '<S1>/Proportional plus integral compensator' */ 589 bh_f14_X->Proportionalplusintegralcompens = 0.0; 590 591 /* InitializeConditions for TransferFcn: '<S4>/W-gust model' */ 592 bh_f14_X->Wgustmodel_CSTATE[0] = 0.0; 593 bh_f14_X->Wgustmodel_CSTATE[1] = 0.0; 594 595 /* InitializeConditions for TransferFcn: '<S4>/Q-gust model' */ 596 bh_f14_X->Qgustmodel_CSTATE = 0.0; 597 598 /* InitializeConditions for RandomNumber: '<S5>/White Noise' */ 599 tmp = floor(23341.0); 600 if (rtIsNaN(tmp) || rtIsInf(tmp)) { 601 tmp = 0.0; 602 } else { 603 tmp = fmod(tmp, 4.294967296E+9); 604 } 605 606 tseed = tmp < 0.0 ? (uint32_T)-(int32_T)(uint32_T)-tmp : (uint32_T)tmp; 607 r = (int32_T)(tseed >> 16U); 608 t = (int32_T)(tseed & 32768U); 609 tseed = ((((tseed - ((uint32_T)r << 16U)) + t) << 16U) + t) + r; 610 if (tseed < 1U) { 611 tseed = 1144108930U; 612 } else { 613 if (tseed > 2147483646U) { 614 tseed = 2147483646U; 615 } 616 } 617 618 bh_f14_DW->RandSeed = tseed; 619 bh_f14_DW->NextOutput = rt_nrand_Upu32_Yd_f_pw_snf(&bh_f14_DW->RandSeed) * 620 1.0 + 0.0; 621 622 /* End of InitializeConditions for RandomNumber: '<S5>/White Noise' */ 623 } 624 } 625 626 /* Model terminate function */ 627 void bh_f14_terminate(RT_MODEL_bh_f14_T *const bh_f14_M) 628 { 629 /* (no terminate code required) */ 630 UNUSED_PARAMETER(bh_f14_M); 631 } 632 633 /* 634 * File trailer for generated code. 635 * 636 * [EOF] 637 */ 638 |