f14.c

matlab实用教程

/*
 * f14.c
 *
 * Real-Time Workshop code generation for Simulink model "f14.mdl".
 *
 * Model Version              : 1.10
 * Real-Time Workshop version : 6.6  (R2007a)  01-Feb-2007
 * C source code generated on : Fri Aug 10 13:58:53 2007
 */

#include "f14.h"
#include "f14_private.h"

/* Block signals (auto storage) */
BlockIO_f14 f14_B;

/* Continuous states */
ContinuousStates_f14 f14_X;

/* Solver Matrices */

/* A and B matrices used by ODE5 fixed-step solver */
static const real_T rt_ODE5_A[6] = {
  1.0/5.0, 3.0/10.0, 4.0/5.0, 8.0/9.0, 1.0, 1.0
};

static const real_T rt_ODE5_B[6][6] = {
  { 1.0/5.0, 0.0, 0.0, 0.0, 0.0, 0.0 },

  { 3.0/40.0, 9.0/40.0, 0.0, 0.0, 0.0, 0.0 },

  { 44.0/45.0, -56.0/15.0, 32.0/9.0, 0.0, 0.0, 0.0 },

  { 19372.0/6561.0, -25360.0/2187.0, 64448.0/6561.0, -212.0/729.0, 0.0, 0.0 },

  { 9017.0/3168.0, -355.0/33.0, 46732.0/5247.0, 49.0/176.0, -5103.0/18656.0, 0.0
  },

  { 35.0/384.0, 0.0, 500.0/1113.0, 125.0/192.0, -2187.0/6784.0, 11.0/84.0 }
};

/* Block states (auto storage) */
D_Work_f14 f14_DWork;

/* External inputs (root inport signals with auto storage) */
ExternalInputs_f14 f14_U;

/* External outputs (root outports fed by signals with auto storage) */
ExternalOutputs_f14 f14_Y;

/* Real-time model */
RT_MODEL_f14 f14_M_;
RT_MODEL_f14 *f14_M = &f14_M_;
static void rate_scheduler(void);

/*
 * This function updates active task flag for each subrate.
 * The function must be is called at model base rate, hence the
 * generated code self-manages all its subrates.
 */
static void rate_scheduler(void)
{
  /* Compute which subrates run during the next base time step.  Subrates
   * are an integer multiple of the base rate counter.  Therefore, the subtask
   * counter is reset when it reaches its limit (zero means run).
   */
  if (++f14_M->Timing.TaskCounters.TID[2] == 2) {/* Sample time: [0.1s, 0.0s] */
    f14_M->Timing.TaskCounters.TID[2] = 0;
  }

  f14_M->Timing.sampleHits[2] = (f14_M->Timing.TaskCounters.TID[2] == 0);
}

/* This function updates continuous states using the ODE5 fixed-step
 * solver algorithm
 */
static void rt_ertODEUpdateContinuousStates(RTWSolverInfo *si )
{
  time_T t = rtsiGetT(si);
  time_T tnew = rtsiGetSolverStopTime(si);
  time_T h = rtsiGetStepSize(si);
  real_T *x = rtsiGetContStates(si);
  ODE5_IntgData *id = (ODE5_IntgData *)rtsiGetSolverData(si);
  real_T *y = id->y;
  real_T *f0 = id->f[0];
  real_T *f1 = id->f[1];
  real_T *f2 = id->f[2];
  real_T *f3 = id->f[3];
  real_T *f4 = id->f[4];
  real_T *f5 = id->f[5];
  real_T hB[6];
  int_T i;
  int_T nXc = 10;
  rtsiSetSimTimeStep(si,MINOR_TIME_STEP);

  /* Save the state values at time t in y, we'll use x as ynew. */
  (void) memcpy(y,x,
                nXc*sizeof(real_T));

  /* Assumes that rtsiSetT and ModelOutputs are up-to-date */
  /* f0 = f(t,y) */
  rtsiSetdX(si, f0);
  f14_derivatives();

  /* f(:,2) = feval(odefile, t + hA(1), y + f*hB(:,1), args(:)(*)); */
  hB[0] = h * rt_ODE5_B[0][0];
  for (i = 0; i < nXc; i++) {
    x[i] = y[i] + (f0[i]*hB[0]);
  }

  rtsiSetT(si, t + h*rt_ODE5_A[0]);
  rtsiSetdX(si, f1);
  f14_output(0);
  f14_derivatives();

  /* f(:,3) = feval(odefile, t + hA(2), y + f*hB(:,2), args(:)(*)); */
  for (i = 0; i <= 1; i++)
    hB[i] = h * rt_ODE5_B[1][i];
  for (i = 0; i < nXc; i++) {
    x[i] = y[i] + (f0[i]*hB[0] + f1[i]*hB[1]);
  }

  rtsiSetT(si, t + h*rt_ODE5_A[1]);
  rtsiSetdX(si, f2);
  f14_output(0);
  f14_derivatives();

  /* f(:,4) = feval(odefile, t + hA(3), y + f*hB(:,3), args(:)(*)); */
  for (i = 0; i <= 2; i++)
    hB[i] = h * rt_ODE5_B[2][i];
  for (i = 0; i < nXc; i++) {
    x[i] = y[i] + (f0[i]*hB[0] + f1[i]*hB[1] + f2[i]*hB[2]);
  }

  rtsiSetT(si, t + h*rt_ODE5_A[2]);
  rtsiSetdX(si, f3);
  f14_output(0);
  f14_derivatives();

  /* f(:,5) = feval(odefile, t + hA(4), y + f*hB(:,4), args(:)(*)); */
  for (i = 0; i <= 3; i++)
    hB[i] = h * rt_ODE5_B[3][i];
  for (i = 0; i < nXc; i++) {
    x[i] = y[i] + (f0[i]*hB[0] + f1[i]*hB[1] + f2[i]*hB[2] +
                   f3[i]*hB[3]);
  }

  rtsiSetT(si, t + h*rt_ODE5_A[3]);
  rtsiSetdX(si, f4);
  f14_output(0);
  f14_derivatives();

  /* f(:,6) = feval(odefile, t + hA(5), y + f*hB(:,5), args(:)(*)); */
  for (i = 0; i <= 4; i++)
    hB[i] = h * rt_ODE5_B[4][i];
  for (i = 0; i < nXc; i++) {
    x[i] = y[i] + (f0[i]*hB[0] + f1[i]*hB[1] + f2[i]*hB[2] +
                   f3[i]*hB[3] + f4[i]*hB[4]);
  }

  rtsiSetT(si, tnew);
  rtsiSetdX(si, f5);
  f14_output(0);
  f14_derivatives();

  /* tnew = t + hA(6);
     ynew = y + f*hB(:,6); */
  for (i = 0; i <= 5; i++)
    hB[i] = h * rt_ODE5_B[5][i];
  for (i = 0; i < nXc; i++) {
    x[i] = y[i] + (f0[i]*hB[0] + f1[i]*hB[1] + f2[i]*hB[2] +
                   f3[i]*hB[3] + f4[i]*hB[4] + f5[i]*hB[5]);
  }

  rtsiSetSimTimeStep(si,MAJOR_TIME_STEP);
}

/* Model output function */
void f14_output(int_T tid)
{
  /* local block i/o variables */
  real_T rtb_Derivative;
  real_T rtb_Derivative1;
  real_T rtb_ActuatorModel;
  real_T rtb_Gain3;
  real_T rtb_Gain4;
  real_T rtb_Gain2;
  real_T rtb_Qgustmodel;
  real_T rtb_AlphasensorLowpassFilter;
  real_T rtb_PitchRateLeadFilter;
  real_T rtb_Gain_f;
  real_T rtb_Proportionalplusintegralcom;
  real_T rtb_WhiteNoise;

  /* Update absolute time of base rate at minor time step */
  if (rtmIsMinorTimeStep(f14_M)) {
    f14_M->Timing.t[0] = rtsiGetT(&f14_M->solverInfo);
  }

  if (rtmIsMajorTimeStep(f14_M)) {
    /* set solver stop time */
    rtsiSetSolverStopTime(&f14_M->solverInfo,
                          ((f14_M->Timing.clockTick0+1)*f14_M->Timing.stepSize0));
  }                                    /* end MajorTimeStep */

  /* TransferFcn Block: '<S1>/Transfer Fcn.2' */
  f14_B.TransferFcn2 = f14_P.TransferFcn2_C*f14_X.TransferFcn2_CSTATE;

  /* Gain: '<Root>/Gain5' */
  f14_B.Gain5 = f14_P.Gain5_Gain * f14_B.TransferFcn2;

  /* Outport: '<Root>/alpha (rad)' */
  f14_Y.alpharad = f14_B.Gain5;

  /* Derivative Block: '<S4>/Derivative' */
  {
    real_T t = f14_M->Timing.t[0];
    real_T timeStampA = f14_DWork.Derivative_RWORK.TimeStampA;
    real_T timeStampB = f14_DWork.Derivative_RWORK.TimeStampB;
    if (timeStampA >= t && timeStampB >= t) {
      rtb_Derivative = 0.0;
    } else {
      real_T deltaT;
      real_T *lastBank = &f14_DWork.Derivative_RWORK.TimeStampA;
      if (timeStampA < timeStampB) {
        if (timeStampB < t) {
          lastBank += 2;
        }
      } else if (timeStampA >= t) {
        lastBank += 2;
      }

      deltaT = t - *lastBank++;
      rtb_Derivative = (f14_B.TransferFcn2 - *lastBank++) / deltaT;
    }
  }

  /* TransferFcn Block: '<S1>/Transfer Fcn.1' */
  f14_B.TransferFcn1 = f14_P.TransferFcn1_C*f14_X.TransferFcn1_CSTATE;

  /* Derivative Block: '<S4>/Derivative1' */
  {
    real_T t = f14_M->Timing.t[0];
    real_T timeStampA = f14_DWork.Derivative1_RWORK.TimeStampA;
    real_T timeStampB = f14_DWork.Derivative1_RWORK.TimeStampB;
    if (timeStampA >= t && timeStampB >= t) {
      rtb_Derivative1 = 0.0;
    } else {
      real_T deltaT;
      real_T *lastBank = &f14_DWork.Derivative1_RWORK.TimeStampA;
      if (timeStampA < timeStampB) {
        if (timeStampB < t) {
          lastBank += 2;
        }
      } else if (timeStampA >= t) {
        lastBank += 2;
      }

      deltaT = t - *lastBank++;
      rtb_Derivative1 = (f14_B.TransferFcn1 - *lastBank++) / deltaT;
    }
  }

  /* Gain: '<S4>/Gain2' incorporates:
   *  Constant: '<S4>/Constant'
   *  Gain: '<S4>/Gain1'
   *  Product: '<S4>/Product'
   *  Sum: '<S4>/Sum1'
   */
  f14_B.Gain2 = (((f14_P.Gain1_Gain * rtb_Derivative1) - rtb_Derivative) +
                 f14_B.TransferFcn1 * f14_P.Constant_Value) * f14_P.Gain2_Gain;

  /* Outport: '<Root>/Nz Pilot (g)' */
  f14_Y.NzPilotg = f14_B.Gain2;
  if (rtmIsMajorTimeStep(f14_M) &&
      f14_M->Timing.TaskCounters.TID[1] == 0) {/* Sample time: [0.05s, 0.0s] */
    /* Scope: '<Root>/Angle of  Attack' */
    if (rtmIsMajorTimeStep(f14_M)) {
      real_T u[2];
      u[0] = f14_M->Timing.t[1];
      u[1] = f14_B.Gain5;
      rt_UpdateLogVar((LogVar *)f14_DWork.AngleofAttack_PWORK.LoggedData, u);
    }

    /* Scope: '<Root>/Pilot G force Scope' */
    if (rtmIsMajorTimeStep(f14_M)) {
      real_T u[2];
      u[0] = f14_M->Timing.t[1];
      u[1] = f14_B.Gain2;
      rt_UpdateLogVar((LogVar *)f14_DWork.PilotGforceScope_PWORK.LoggedData, u);
    }
  }

  /* SignalGenerator: '<Root>/Pilot' */
  {
    real_T phase = f14_P.Pilot_Frequency*f14_M->Timing.t[0];
    phase = phase-floor(phase);
    rtb_Proportionalplusintegralcom = ( phase >= 0.5 ) ?
      f14_P.Pilot_Amplitude : -f14_P.Pilot_Amplitude;
  }

  /* Sum: '<Root>/Sum1' incorporates:
   *  Inport: '<Root>/u'
   */
  f14_B.Sum1 = rtb_Proportionalplusintegralcom + f14_U.u;
  if (rtmIsMajorTimeStep(f14_M) &&
      f14_M->Timing.TaskCounters.TID[1] == 0) {/* Sample time: [0.05s, 0.0s] */
    /* Scope: '<Root>/Stick Input' */
    if (rtmIsMajorTimeStep(f14_M)) {
      real_T u[2];
      u[0] = f14_M->Timing.t[1];
      u[1] = f14_B.Sum1;
      rt_UpdateLogVar((LogVar *)f14_DWork.StickInput_PWORK.LoggedData, u);
    }
  }

  /* TransferFcn Block: '<Root>/Actuator Model' */
  rtb_ActuatorModel = f14_P.ActuatorModel_C*f14_X.ActuatorModel_CSTATE;

  /* Gain: '<S1>/Gain3' */
  rtb_Gain3 = f14_P.Gain3_Gain * f14_B.TransferFcn1;

  /* Gain: '<S1>/Gain4' */
  rtb_Gain4 = f14_P.Gain4_Gain * f14_B.TransferFcn2;

  /* TransferFcn Block: '<S3>/W-gust model' */
  f14_B.Wgustmodel = f14_P.Wgustmodel_C[0]*f14_X.Wgustmodel_CSTATE[0] +
    f14_P.Wgustmodel_C[1]*f14_X.Wgustmodel_CSTATE[1];

  /* Sum: '<S1>/Sum1' incorporates:
   *  Gain: '<Root>/Gain'
   *  Gain: '<S1>/Gain5'
   */
  f14_B.Sum1_b = ((f14_P.Gain5_Gain_k * rtb_ActuatorModel) - f14_P.Gain_Gain *
                  f14_B.Wgustmodel) + rtb_Gain3;

  /* Gain: '<Root>/Gain2' */
  rtb_Gain2 = f14_P.Gain2_Gain_c * f14_B.Wgustmodel;

  /* TransferFcn Block: '<S3>/Q-gust model' */
  rtb_Qgustmodel = f14_P.Qgustmodel_D*f14_B.Wgustmodel;
  rtb_Qgustmodel += f14_P.Qgustmodel_C*f14_X.Qgustmodel_CSTATE;

  /* Sum: '<S1>/Sum2' incorporates:
   *  Gain: '<Root>/Gain1'
   *  Gain: '<S1>/Gain6'
   *  Sum: '<Root>/Sum'
   */
  f14_B.Sum2 = (rtb_Gain4 - (f14_P.Gain1_Gain_e * rtb_Qgustmodel + rtb_Gain2)) +
    (f14_P.Gain6_Gain * rtb_ActuatorModel);

  /* TransferFcn Block: '<S2>/Alpha-sensor Low-pass Filter' */
  rtb_AlphasensorLowpassFilter = f14_P.AlphasensorLowpassFilt_C*
    f14_X.AlphasensorLowpassFilter_CSTATE;

  /* TransferFcn Block: '<S2>/Stick Prefilter' */
  rtb_Proportionalplusintegralcom = f14_P.StickPrefilter_C*
    f14_X.StickPrefilter_CSTATE;

  /* TransferFcn Block: '<S2>/Pitch Rate Lead Filter' */
  rtb_PitchRateLeadFilter = f14_P.PitchRateLeadFilter_D*f14_B.TransferFcn1;
  rtb_PitchRateLeadFilter += f14_P.PitchRateLeadFilter_C*
    f14_X.PitchRateLeadFilter_CSTATE;

  /* Sum: '<S2>/Sum2' incorporates:
   *  Gain: '<S2>/Gain2'
   *  Gain: '<S2>/Gain3'
   *  Sum: '<S2>/Sum1'
   */
  f14_B.Sum2_f = rtb_Proportionalplusintegralcom - (f14_P.Gain2_Gain_n *
    rtb_PitchRateLeadFilter + f14_P.Gain3_Gain_k * rtb_AlphasensorLowpassFilter);

  /* Gain: '<S2>/Gain' */
  rtb_Gain_f = f14_P.Gain_Gain_a * f14_B.Sum2_f;

  /* TransferFcn Block: '<S2>/Proportional plus integral compensator' */