예제 #1
0
/* Model output function */
void m1006_output(int_T tid)
{

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

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

  /* Sin: '<Root>/Sine Wave' */
  m1006_B.SineWave = m1006_P.SineWave_Amp *
    sin(m1006_P.SineWave_Freq * m1006_M->Timing.t[0] + m1006_P.SineWave_Phase) +
    m1006_P.SineWave_Bias;

  /* S-Function "motor_wrapper" Block: <Root>/S-Function Builder */

  {
    real_T *pxc = m1006_M->ModelData.contStates;
    motor_Outputs_wrapper(&m1006_B.SineWave, &m1006_B.SFunctionBuilder, pxc);
  }

  /* Outport: '<Root>/Out1' */
  m1006_Y.Out1 = m1006_B.SFunctionBuilder;
}
예제 #2
0
/* This function updates continuous states using the ODE4 fixed-step
 * solver algorithm
 */
static void rt_ertODEUpdateContinuousStates(RTWSolverInfo *si , int_T tid)
{
  time_T t = rtsiGetT(si);
  time_T tnew = rtsiGetSolverStopTime(si);
  time_T h = rtsiGetStepSize(si);
  real_T *x = rtsiGetContStates(si);
  ODE4_IntgData *id = 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 temp;
  int_T i;

  int_T nXc = 1;

  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);
  m1006_derivatives();

  /* f1 = f(t + (h/2), y + (h/2)*f0) */
  temp = 0.5 * h;
  for (i = 0; i < nXc; i++) x[i] = y[i] + (temp*f0[i]);
  rtsiSetT(si, t + temp);
  rtsiSetdX(si, f1);
  m1006_output(0);
  m1006_derivatives();

  /* f2 = f(t + (h/2), y + (h/2)*f1) */
  for (i = 0; i < nXc; i++) x[i] = y[i] + (temp*f1[i]);
  rtsiSetdX(si, f2);
  m1006_output(0);
  m1006_derivatives();

  /* f3 = f(t + h, y + h*f2) */
  for (i = 0; i < nXc; i++) x[i] = y[i] + (h*f2[i]);
  rtsiSetT(si, tnew);
  rtsiSetdX(si, f3);
  m1006_output(0);
  m1006_derivatives();

  /* tnew = t + h
     ynew = y + (h/6)*(f0 + 2*f1 + 2*f2 + 2*f3) */
  temp = h / 6.0;
  for (i = 0; i < nXc; i++) {
    x[i] = y[i] + temp*(f0[i] + 2.0*f1[i] + 2.0*f2[i] + f3[i]);
  }

  rtsiSetSimTimeStep(si,MAJOR_TIME_STEP);
}
예제 #3
0
/* Model output function */
void xpcosc_output(int_T tid)
{
  real_T temp;
  if (rtmIsMajorTimeStep(xpcosc_rtM)) {
    /* set solver stop time */
    if (!(xpcosc_rtM->Timing.clockTick0+1)) {
      rtsiSetSolverStopTime(&xpcosc_rtM->solverInfo,
                            ((xpcosc_rtM->Timing.clockTickH0 + 1) *
        xpcosc_rtM->Timing.stepSize0 * 4294967296.0));
    } else {
      rtsiSetSolverStopTime(&xpcosc_rtM->solverInfo,
                            ((xpcosc_rtM->Timing.clockTick0 + 1) *
        xpcosc_rtM->Timing.stepSize0 + xpcosc_rtM->Timing.clockTickH0 *
        xpcosc_rtM->Timing.stepSize0 * 4294967296.0));
    }
  }                                    /* end MajorTimeStep */

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

  /* Integrator: '<Root>/Integrator1' */
  xpcosc_B.Integrator1 = xpcosc_X.Integrator1_CSTATE;
  if (rtmIsMajorTimeStep(xpcosc_rtM)) {
    /* Level2 S-Function Block: '<Root>/PCI-6221 AD' (adnipcim) */
    {
      SimStruct *rts = xpcosc_rtM->childSfunctions[0];
      sfcnOutputs(rts, 1);
    }

    /* RateTransition: '<Root>/Rate Transition1' */
    if (rtmIsMajorTimeStep(xpcosc_rtM)) {
      xpcosc_B.RateTransition1 = xpcosc_B.PCI6221AD;
    }

    /* End of RateTransition: '<Root>/Rate Transition1' */
  }

  /* Outport: '<Root>/Outport' */
  xpcosc_Y.Outport[0] = xpcosc_B.Integrator1;
  xpcosc_Y.Outport[1] = xpcosc_B.RateTransition1;

  /* SignalGenerator: '<Root>/Signal Generator' */
  temp = xpcosc_P.SignalGenerator_Frequency * xpcosc_rtM->Timing.t[0];
  if (temp - floor(temp) >= 0.5) {
    xpcosc_B.SignalGenerator = xpcosc_P.SignalGenerator_Amplitude;
  } else {
    xpcosc_B.SignalGenerator = -xpcosc_P.SignalGenerator_Amplitude;
  }

  /* End of SignalGenerator: '<Root>/Signal Generator' */

  /* RateTransition: '<Root>/Rate Transition' */
  if (rtmIsMajorTimeStep(xpcosc_rtM)) {
    xpcosc_B.RateTransition = xpcosc_B.SignalGenerator;

    /* Level2 S-Function Block: '<Root>/PCI-6713 DA' (danipci671x) */
    {
      SimStruct *rts = xpcosc_rtM->childSfunctions[1];
      sfcnOutputs(rts, 1);
    }
  }

  /* End of RateTransition: '<Root>/Rate Transition' */
  /* ok to acquire for <S1>/S-Function */
  xpcosc_DWork.SFunction_IWORK.AcquireOK = 1;

  /* Gain: '<Root>/Gain' */
  xpcosc_B.Gain = xpcosc_P.Gain_Gain * xpcosc_B.Integrator1;

  /* Integrator: '<Root>/Integrator' */
  xpcosc_B.Integrator = xpcosc_X.Integrator_CSTATE;

  /* Gain: '<Root>/Gain1' */
  xpcosc_B.Gain1 = xpcosc_P.Gain1_Gain * xpcosc_B.Integrator;

  /* Gain: '<Root>/Gain2' */
  xpcosc_B.Gain2 = xpcosc_P.Gain2_Gain * xpcosc_B.SignalGenerator;

  /* Sum: '<Root>/Sum' */
  xpcosc_B.Sum = (xpcosc_B.Gain2 - xpcosc_B.Gain) - xpcosc_B.Gain1;

  /* tid is required for a uniform function interface.
   * Argument tid is not used in the function. */
  UNUSED_PARAMETER(tid);
}
예제 #4
0
/* Model output function */
void Crane_output(int_T tid)
{
  /* local block i/o variables */
  real_T rtb_Falcon_o1[3];
  real_T rtb_Gain4[3];
  boolean_T rtb_Falcon_o2[4];

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

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

  {
    real_T rtb_Product_idx;
    real_T rtb_Product_idx_0;
    real_T rtb_Product_idx_1;
    real_T rtb_Product1_idx;
    real_T rtb_Product1_idx_0;
    real_T rtb_Product1_idx_1;

    /* S-Function Block: <S10>/Block#1 */
    {
      _rtMech_PWORK *mechWork = (_rtMech_PWORK *) Crane_DWork.Block1_PWORK;
      mechWork->genSimData.time = Crane_M->Timing.t[0];
      mechWork->outSimData.majorTimestep = rtmIsMajorTimeStep(Crane_M);
      if (kinematicSfcnOutputMethod(mechWork->mechanism, &(mechWork->genSimData),
           &(mechWork->outSimData))) {
        {
          const ErrorRecord *err = getErrorMsg();
          static char_T errorMsg[1024];
          sprintf(errorMsg,
                  err->errorMsg,
                  err->blocks[0],
                  err->blocks[1],
                  err->blocks[2],
                  err->blocks[3],
                  err->blocks[4]);
          rtmSetErrorStatus(Crane_M, errorMsg);
          return;
        }
      }
    }

    /* Gain: '<S2>/gain_1' */
    Crane_B.gain_1[0] = Crane_P.gain_1_Gain * Crane_B.Block1_o1[0];
    Crane_B.gain_1[1] = Crane_P.gain_1_Gain * Crane_B.Block1_o1[1];
    Crane_B.gain_1[2] = Crane_P.gain_1_Gain * Crane_B.Block1_o1[2];
    if (rtmIsMajorTimeStep(Crane_M) &&
        Crane_M->Timing.TaskCounters.TID[1] == 0) {
      /* Memory: '<Root>/Memory' */
      Crane_B.Memory[0] = Crane_DWork.Memory_PreviousInput[0];
      Crane_B.Memory[1] = Crane_DWork.Memory_PreviousInput[1];
      Crane_B.Memory[2] = Crane_DWork.Memory_PreviousInput[2];
    }

    /* Sum: '<S9>/Sum' */
    Crane_B.Sum[0] = Crane_B.Memory[0] - Crane_B.gain_1[0];
    Crane_B.Sum[1] = Crane_B.Memory[1] - Crane_B.gain_1[1];
    Crane_B.Sum[2] = Crane_B.Memory[2] - Crane_B.gain_1[2];

    /* Product: '<S9>/Product' incorporates:
     *  Constant: '<Root>/Constant2'
     */
    rtb_Product_idx = Crane_P.Constant2_Value * Crane_B.Sum[0];
    rtb_Product_idx_0 = Crane_P.Constant2_Value * Crane_B.Sum[1];
    rtb_Product_idx_1 = Crane_P.Constant2_Value * Crane_B.Sum[2];

    /* Integrator: '<S9>/Integrator' */
    rtb_Gain4[0] = Crane_X.Integrator_CSTATE[0];
    rtb_Gain4[1] = Crane_X.Integrator_CSTATE[1];
    rtb_Gain4[2] = Crane_X.Integrator_CSTATE[2];

    /* Product: '<S9>/Product1' incorporates:
     *  Constant: '<Root>/Constant5'
     */
    rtb_Product1_idx = Crane_P.Constant5_Value * rtb_Gain4[0];
    rtb_Product1_idx_0 = Crane_P.Constant5_Value * rtb_Gain4[1];
    rtb_Product1_idx_1 = Crane_P.Constant5_Value * rtb_Gain4[2];

    /* Derivative Block: '<S9>/Derivative' */
    {
      real_T t = Crane_M->Timing.t[0];
      real_T timeStampA = Crane_DWork.Derivative_RWORK.TimeStampA;
      real_T timeStampB = Crane_DWork.Derivative_RWORK.TimeStampB;
      if (timeStampA >= t && timeStampB >= t) {
        rtb_Gain4[0] = 0.0;
        rtb_Gain4[1] = 0.0;
        rtb_Gain4[2] = 0.0;
      } else {
        real_T deltaT;
        real_T *lastBank = &Crane_DWork.Derivative_RWORK.TimeStampA;
        if (timeStampA < timeStampB) {
          if (timeStampB < t) {
            lastBank += 4;
          }
        } else if (timeStampA >= t) {
          lastBank += 4;
        }

        deltaT = t - *lastBank++;
        rtb_Gain4[0] = (Crane_B.Sum[0] - *lastBank++) / deltaT;
        rtb_Gain4[1] = (Crane_B.Sum[1] - *lastBank++) / deltaT;
        rtb_Gain4[2] = (Crane_B.Sum[2] - *lastBank++) / deltaT;
      }
    }

    /* Sum: '<S9>/Sum1' incorporates:
     *  Constant: '<Root>/Constant6'
     *  Product: '<S9>/Product2'
     */
    Crane_B.Sum1[0] = (rtb_Product_idx + rtb_Product1_idx) +
      Crane_P.Constant6_Value * rtb_Gain4[0];
    Crane_B.Sum1[1] = (rtb_Product_idx_0 + rtb_Product1_idx_0) +
      Crane_P.Constant6_Value * rtb_Gain4[1];
    Crane_B.Sum1[2] = (rtb_Product_idx_1 + rtb_Product1_idx_1) +
      Crane_P.Constant6_Value * rtb_Gain4[2];

    /* Gain: '<Root>/Gain4' */
    rtb_Gain4[0] = Crane_P.Gain4_Gain * Crane_B.Sum1[0];
    rtb_Gain4[1] = Crane_P.Gain4_Gain * Crane_B.Sum1[1];
    rtb_Gain4[2] = Crane_P.Gain4_Gain * Crane_B.Sum1[2];

    /* Saturate: '<Root>/Saturation' */
    Crane_B.Saturation[0] = rt_SATURATE(rtb_Gain4[2],
      Crane_P.Saturation_LowerSat, Crane_P.Saturation_UpperSat);
    Crane_B.Saturation[1] = rt_SATURATE(rtb_Gain4[0],
      Crane_P.Saturation_LowerSat, Crane_P.Saturation_UpperSat);
    Crane_B.Saturation[2] = rt_SATURATE(rtb_Gain4[1],
      Crane_P.Saturation_LowerSat, Crane_P.Saturation_UpperSat);
    if (rtmIsMajorTimeStep(Crane_M) &&
        Crane_M->Timing.TaskCounters.TID[1] == 0) {
      /* S-Function Block: Crane/Falcon (falcon_block) */
      {
        t_error result;
        double force_vector[3];
        double position[3];
        t_int read_buttons;
        force_vector[0] = Crane_B.Saturation[0];
        force_vector[1] = Crane_B.Saturation[1];
        force_vector[2] = Crane_B.Saturation[2];

        /* read the position and buttons, and output the requested force to the falcon */
        result = falcon_read_write(Crane_DWork.Falcon_Falcon, position,
          &read_buttons, force_vector);
        if (result < 0) {
          msg_get_error_messageA(NULL, result, _rt_error_message, sizeof
            (_rt_error_message));
          rtmSetErrorStatus(Crane_M, _rt_error_message);
          return;
        }

        rtb_Falcon_o1[0] = position[0];
        rtb_Falcon_o1[1] = position[1];
        rtb_Falcon_o1[2] = position[2];
        rtb_Falcon_o2[0] = ((read_buttons & 0x01) != 0);
        rtb_Falcon_o2[1] = ((read_buttons & 0x02) != 0);
        rtb_Falcon_o2[2] = ((read_buttons & 0x04) != 0);
        rtb_Falcon_o2[3] = ((read_buttons & 0x08) != 0);
      }

      /* Gain: '<Root>/Gain3' */
      Crane_B.Gain3[0] = Crane_P.Gain3_Gain * rtb_Falcon_o1[0];
      Crane_B.Gain3[1] = Crane_P.Gain3_Gain * rtb_Falcon_o1[1];
      Crane_B.Gain3[2] = Crane_P.Gain3_Gain * rtb_Falcon_o1[2];

      /* Constant: '<Root>/Constant' */
      Crane_B.Constant = Crane_P.Constant_Value;

      /* Stop: '<Root>/Stop Simulation' */
      if (rtb_Falcon_o2[0] || rtb_Falcon_o2[1] || rtb_Falcon_o2[2] ||
          rtb_Falcon_o2[3]) {
        rtmSetStopRequested(Crane_M, 1);
      }

      /* Gain: '<Root>/Gain2' incorporates:
       *  Constant: '<Root>/Constant1'
       */
      Crane_B.Gain2 = Crane_P.Gain2_Gain * Crane_P.Constant1_Value;

      /* Gain: '<Root>/Gain1' incorporates:
       *  Constant: '<Root>/Constant3'
       */
      Crane_B.Gain1 = Crane_P.Gain1_Gain * Crane_P.Constant3_Value;
    }

    /* Gain: '<Root>/Gain' incorporates:
     *  Sin: '<Root>/Sine Wave'
     */
    Crane_B.Gain = (sin(Crane_P.SineWave_Freq * Crane_M->Timing.t[0] +
                        Crane_P.SineWave_Phase) * Crane_P.SineWave_Amp +
                    Crane_P.SineWave_Bias) * Crane_P.Gain_Gain;
    if (rtmIsMajorTimeStep(Crane_M) &&
        Crane_M->Timing.TaskCounters.TID[1] == 0) {
      /* Gain: '<S10>/_gravity_conversion' incorporates:
       *  Constant: '<S8>/SOURCE_BLOCK'
       */
      Crane_B._gravity_conversion[0] = Crane_P._gravity_conversion_Gain *
        Crane_P.SOURCE_BLOCK_Value[0];
      Crane_B._gravity_conversion[1] = Crane_P._gravity_conversion_Gain *
        Crane_P.SOURCE_BLOCK_Value[1];
      Crane_B._gravity_conversion[2] = Crane_P._gravity_conversion_Gain *
        Crane_P.SOURCE_BLOCK_Value[2];
    }

    /* S-Function Block: <S10>/Block#2 */
    {
      _rtMech_PWORK *mechWork = (_rtMech_PWORK *) Crane_DWork.Block2_PWORK;
      mechWork->genSimData.time = Crane_M->Timing.t[0];
      mechWork->outSimData.majorTimestep = rtmIsMajorTimeStep(Crane_M);
      if (dynamicSfcnOutputMethod(mechWork->mechanism, &(mechWork->genSimData),
           &(mechWork->outSimData))) {
        {
          const ErrorRecord *err = getErrorMsg();
          static char_T errorMsg[1024];
          sprintf(errorMsg,
                  err->errorMsg,
                  err->blocks[0],
                  err->blocks[1],
                  err->blocks[2],
                  err->blocks[3],
                  err->blocks[4]);
          rtmSetErrorStatus(Crane_M, errorMsg);
          return;
        }
      }
    }

    /* S-Function Block: <S10>/Block#3 */
    {
      _rtMech_PWORK *mechWork = (_rtMech_PWORK *) Crane_DWork.Block3_PWORK;
      mechWork->genSimData.time = Crane_M->Timing.t[0];
      mechWork->outSimData.majorTimestep = rtmIsMajorTimeStep(Crane_M);
      if (eventSfcnOutputMethod(mechWork->mechanism, &(mechWork->genSimData),
           &(mechWork->outSimData))) {
        {
          const ErrorRecord *err = getErrorMsg();
          static char_T errorMsg[1024];
          sprintf(errorMsg,
                  err->errorMsg,
                  err->blocks[0],
                  err->blocks[1],
                  err->blocks[2],
                  err->blocks[3],
                  err->blocks[4]);
          rtmSetErrorStatus(Crane_M, errorMsg);
          return;
        }
      }
    }
  }

  UNUSED_PARAMETER(tid);
}
예제 #5
0
/* Model output function */
void motor_io_position_output(void)
{
  real_T temp;
  ZCEventType zcEvent;
  real_T u1;
  real_T u2;
  if (rtmIsMajorTimeStep(motor_io_position_M)) {
    /* set solver stop time */
    if (!(motor_io_position_M->Timing.clockTick0+1)) {
      rtsiSetSolverStopTime(&motor_io_position_M->solverInfo,
                            ((motor_io_position_M->Timing.clockTickH0 + 1) *
        motor_io_position_M->Timing.stepSize0 * 4294967296.0));
    } else {
      rtsiSetSolverStopTime(&motor_io_position_M->solverInfo,
                            ((motor_io_position_M->Timing.clockTick0 + 1) *
        motor_io_position_M->Timing.stepSize0 +
        motor_io_position_M->Timing.clockTickH0 *
        motor_io_position_M->Timing.stepSize0 * 4294967296.0));
    }
  }                                    /* end MajorTimeStep */

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

  if (rtmIsMajorTimeStep(motor_io_position_M)) {
    /* S-Function (rti_commonblock): '<S13>/S-Function1' */
    /* This comment workarounds a code generation problem */

    /* Gain: '<S5>/fi1_scaling' */
    motor_io_position_B.fi1_scaling = motor_io_position_P.fi1_scaling_Gain *
      motor_io_position_B.SFunction1;

    /* DiscreteTransferFcn: '<Root>/Gfbreal' */
    temp = motor_io_position_B.fi1_scaling;
    temp -= motor_io_position_P.Gfbreal_DenCoef[1] *
      motor_io_position_DW.Gfbreal_states[0];
    temp -= motor_io_position_P.Gfbreal_DenCoef[2] *
      motor_io_position_DW.Gfbreal_states[1];
    temp /= motor_io_position_P.Gfbreal_DenCoef[0];
    motor_io_position_DW.Gfbreal_tmp = temp;
    temp = motor_io_position_P.Gfbreal_NumCoef[0] *
      motor_io_position_DW.Gfbreal_tmp;
    temp += motor_io_position_P.Gfbreal_NumCoef[1] *
      motor_io_position_DW.Gfbreal_states[0];
    temp += motor_io_position_P.Gfbreal_NumCoef[2] *
      motor_io_position_DW.Gfbreal_states[1];
    motor_io_position_B.Gfbreal = temp;
  }

  /* SignalGenerator: '<Root>/SinGenerator' */
  motor_io_position_B.SinGenerator = sin
    (motor_io_position_P.SinGenerator_Frequency * motor_io_position_M->Timing.t
     [0]) * motor_io_position_P.SinGenerator_Amplitude;

  /* SignalGenerator: '<Root>/SquareGenerator' */
  temp = motor_io_position_P.SquareGenerator_Frequency *
    motor_io_position_M->Timing.t[0];
  if (temp - floor(temp) >= 0.5) {
    motor_io_position_B.SquareGenerator =
      motor_io_position_P.SquareGenerator_Amplitude;
  } else {
    motor_io_position_B.SquareGenerator =
      -motor_io_position_P.SquareGenerator_Amplitude;
  }

  /* End of SignalGenerator: '<Root>/SquareGenerator' */

  /* Switch: '<Root>/Switch' incorporates:
   *  Constant: '<Root>/SignalSelector[0Square,1Sine]'
   */
  if (motor_io_position_P.SignalSelector0Square1Sine_Valu != 0.0) {
    motor_io_position_B.ref = motor_io_position_B.SinGenerator;
  } else {
    motor_io_position_B.ref = motor_io_position_B.SquareGenerator;
  }

  /* End of Switch: '<Root>/Switch' */
  if (rtmIsMajorTimeStep(motor_io_position_M)) {
    /* DiscreteTransferFcn: '<Root>/Gffreal' */
    temp = motor_io_position_B.ref;
    temp -= motor_io_position_P.Gffreal_DenCoef[1] *
      motor_io_position_DW.Gffreal_states[0];
    temp -= motor_io_position_P.Gffreal_DenCoef[2] *
      motor_io_position_DW.Gffreal_states[1];
    temp /= motor_io_position_P.Gffreal_DenCoef[0];
    motor_io_position_DW.Gffreal_tmp = temp;
    temp = motor_io_position_P.Gffreal_NumCoef[0] *
      motor_io_position_DW.Gffreal_tmp;
    temp += motor_io_position_P.Gffreal_NumCoef[1] *
      motor_io_position_DW.Gffreal_states[0];
    temp += motor_io_position_P.Gffreal_NumCoef[2] *
      motor_io_position_DW.Gffreal_states[1];
    motor_io_position_B.Gffreal = temp;

    /* Sum: '<Root>/Sum' */
    motor_io_position_B.Sum = motor_io_position_B.Gffreal -
      motor_io_position_B.Gfbreal;

    /* Gain: '<Root>/Gain' */
    motor_io_position_B.Gain = motor_io_position_P.Gain_Gain *
      motor_io_position_B.Sum;

    /* Saturate: '<S2>/Saturation' */
    temp = motor_io_position_B.Gain;
    u1 = motor_io_position_P.Saturation_LowerSat;
    u2 = motor_io_position_P.Saturation_UpperSat;
    if (temp > u2) {
      motor_io_position_B.Volt = u2;
    } else if (temp < u1) {
      motor_io_position_B.Volt = u1;
    } else {
      motor_io_position_B.Volt = temp;
    }

    /* End of Saturate: '<S2>/Saturation' */

    /* Gain: '<S2>/pwm_skalning' */
    motor_io_position_B.pwm_skalning = motor_io_position_P.pwm_skalning_Gain *
      motor_io_position_B.Volt;

    /* Sum: '<S2>/Sum' incorporates:
     *  Constant: '<S2>/pwm_offstet'
     */
    motor_io_position_B.Sum_f = motor_io_position_B.pwm_skalning +
      motor_io_position_P.pwm_offstet_Value;

    /* S-Function (rti_commonblock): '<S9>/S-Function1' */
    /* This comment workarounds a code generation problem */

    /* dSPACE I/O Board DS1104 #1 Unit:PWM Group:PWM */
    ds1104_slave_dsp_pwm_duty_write(0, rti_slv1104_fcn_index[6],
      motor_io_position_B.Sum_f);

    /* S-Function (rti_commonblock): '<S9>/S-Function2' */
    /* This comment workarounds a code generation problem */

    /* S-Function (rti_commonblock): '<S9>/S-Function3' */
    /* This comment workarounds a code generation problem */

    /* S-Function (rti_commonblock): '<S9>/S-Function4' */
    /* This comment workarounds a code generation problem */

    /* DataTypeConversion: '<S2>/Data Type Conversion' incorporates:
     *  Constant: '<S2>/Enable[1_Off, 0_On]'
     */
    motor_io_position_B.DataTypeConversion =
      (motor_io_position_P.Enable1_Off0_On_Value != 0.0);

    /* S-Function (rti_commonblock): '<S8>/S-Function1' */
    /* This comment workarounds a code generation problem */

    /* dSPACE I/O Board DS1104 #1 Unit:BIT_IO Group:BIT_OUT */
    if (motor_io_position_B.DataTypeConversion > 0) {
      ds1104_bit_io_set(DS1104_DIO0);
    } else {
      ds1104_bit_io_clear(DS1104_DIO0);
    }

    /* DiscreteTransferFcn: '<Root>/Gff' */
    temp = motor_io_position_B.ref;
    temp -= motor_io_position_P.Gff_DenCoef[1] *
      motor_io_position_DW.Gff_states[0];
    temp -= motor_io_position_P.Gff_DenCoef[2] *
      motor_io_position_DW.Gff_states[1];
    temp /= motor_io_position_P.Gff_DenCoef[0];
    motor_io_position_DW.Gff_tmp = temp;
    temp = motor_io_position_P.Gff_NumCoef[0] * motor_io_position_DW.Gff_tmp;
    temp += motor_io_position_P.Gff_NumCoef[1] *
      motor_io_position_DW.Gff_states[0];
    temp += motor_io_position_P.Gff_NumCoef[2] *
      motor_io_position_DW.Gff_states[1];
    motor_io_position_B.Gff = temp;
  }

  /* Integrator: '<S1>/Integrator1' */
  motor_io_position_B.Integrator1 = motor_io_position_X.Integrator1_CSTATE;

  /* Quantizer: '<S4>/Quantizer' */
  temp = motor_io_position_B.Integrator1;
  motor_io_position_B.Quantizer = rt_roundd_snf(temp / motor_io_position_P.quant)
    * motor_io_position_P.quant;
  if (rtmIsMajorTimeStep(motor_io_position_M)) {
    /* ZeroOrderHold: '<S4>/Zero-Order Hold' */
    motor_io_position_B.ZeroOrderHold = motor_io_position_B.Quantizer;

    /* DiscreteTransferFcn: '<Root>/Gfb' */
    temp = motor_io_position_B.ZeroOrderHold;
    temp -= motor_io_position_P.Gfb_DenCoef[1] *
      motor_io_position_DW.Gfb_states[0];
    temp -= motor_io_position_P.Gfb_DenCoef[2] *
      motor_io_position_DW.Gfb_states[1];
    temp /= motor_io_position_P.Gfb_DenCoef[0];
    motor_io_position_DW.Gfb_tmp = temp;
    temp = motor_io_position_P.Gfb_NumCoef[0] * motor_io_position_DW.Gfb_tmp;
    temp += motor_io_position_P.Gfb_NumCoef[1] *
      motor_io_position_DW.Gfb_states[0];
    temp += motor_io_position_P.Gfb_NumCoef[2] *
      motor_io_position_DW.Gfb_states[1];
    motor_io_position_B.Gfb = temp;

    /* Sum: '<Root>/Sum1' */
    motor_io_position_B.Sum1 = motor_io_position_B.Gff - motor_io_position_B.Gfb;

    /* Saturate: '<Root>/Saturation' */
    temp = motor_io_position_B.Sum1;
    u1 = motor_io_position_P.Saturation_LowerSat_c;
    u2 = motor_io_position_P.Saturation_UpperSat_p;
    if (temp > u2) {
      motor_io_position_B.Saturation = u2;
    } else if (temp < u1) {
      motor_io_position_B.Saturation = u1;
    } else {
      motor_io_position_B.Saturation = temp;
    }

    /* End of Saturate: '<Root>/Saturation' */
  }

  /* Integrator: '<S1>/Integrator' */
  motor_io_position_B.Integrator = motor_io_position_X.Integrator_CSTATE;

  /* Gain: '<S1>/Gain1' */
  motor_io_position_B.Gain1 = motor_io_position_P.Gain1_Gain *
    motor_io_position_B.Integrator;

  /* Sum: '<S1>/Add' */
  motor_io_position_B.Add = motor_io_position_B.Saturation -
    motor_io_position_B.Gain1;

  /* Gain: '<S1>/k//R ' */
  motor_io_position_B.kR = motor_io_position_P.kR_Gain * motor_io_position_B.Add;

  /* Saturate: '<S6>/Saturate to Fc' */
  temp = motor_io_position_B.kR;
  u1 = motor_io_position_P.SaturatetoFc_LowerSat;
  u2 = motor_io_position_P.F_c;
  if (temp > u2) {
    motor_io_position_B.Stickslipregion = u2;
  } else if (temp < u1) {
    motor_io_position_B.Stickslipregion = u1;
  } else {
    motor_io_position_B.Stickslipregion = temp;
  }

  /* End of Saturate: '<S6>/Saturate to Fc' */

  /* Abs: '<S6>/Abs' */
  motor_io_position_B.Abs = fabs(motor_io_position_B.Integrator);

  /* RelationalOperator: '<S7>/Compare' incorporates:
   *  Constant: '<S7>/Constant'
   */
  motor_io_position_B.Compare = (motor_io_position_B.Abs <=
    motor_io_position_P.Constant_Value);

  /* Gain: '<S6>/Vicous friction' */
  motor_io_position_B.Vicousfriction = motor_io_position_P.Vicousfriction_Gain *
    motor_io_position_B.Integrator;

  /* Signum: '<S6>/Sign' */
  temp = motor_io_position_B.Integrator;
  if (temp < 0.0) {
    motor_io_position_B.Sign = -1.0;
  } else if (temp > 0.0) {
    motor_io_position_B.Sign = 1.0;
  } else if (temp == 0.0) {
    motor_io_position_B.Sign = 0.0;
  } else {
    motor_io_position_B.Sign = temp;
  }

  /* End of Signum: '<S6>/Sign' */

  /* Product: '<S6>/Product' incorporates:
   *  Constant: '<S6>/F_c'
   */
  motor_io_position_B.Product = motor_io_position_P.F_c *
    motor_io_position_B.Sign;

  /* Sum: '<S6>/Add' */
  motor_io_position_B.Viscousregion = motor_io_position_B.Vicousfriction +
    motor_io_position_B.Product;

  /* Switch: '<S6>/Switch' */
  if (motor_io_position_B.Compare) {
    motor_io_position_B.Friction = motor_io_position_B.Stickslipregion;
  } else {
    motor_io_position_B.Friction = motor_io_position_B.Viscousregion;
  }

  /* End of Switch: '<S6>/Switch' */

  /* Sum: '<S1>/Add1' */
  motor_io_position_B.Add1 = motor_io_position_B.kR -
    motor_io_position_B.Friction;
  if (rtmIsMajorTimeStep(motor_io_position_M)) {
    /* Gain: '<S1>/Gain2' incorporates:
     *  Constant: '<S1>/Load inertia'
     */
    motor_io_position_B.Gain2 = motor_io_position_P.Gain2_Gain *
      motor_io_position_P.J1;

    /* Sum: '<S1>/Add2' incorporates:
     *  Constant: '<S1>/Motor inertia'
     */
    motor_io_position_B.Add2 = motor_io_position_B.Gain2 +
      motor_io_position_P.Motorinertia_Value;
  }

  /* Product: '<S1>/Inertias 1//J' */
  motor_io_position_B.Inertias1J = 1.0 / motor_io_position_B.Add2 *
    motor_io_position_B.Add1;
  if (rtmIsMajorTimeStep(motor_io_position_M)) {
  }

  /* Switch: '<S6>/Switch1' incorporates:
   *  Constant: '<S6>/Constant1'
   *  Constant: '<S6>/F_c'
   */
  if (motor_io_position_B.Integrator > motor_io_position_P.Switch1_Threshold) {
    motor_io_position_B.Switch1 = motor_io_position_P.F_c;
  } else {
    motor_io_position_B.Switch1 = motor_io_position_P.Constant1_Value;
  }

  /* End of Switch: '<S6>/Switch1' */
  if (rtmIsMajorTimeStep(motor_io_position_M)) {
    /* S-Function (rti_commonblock): '<S13>/S-Function2' */
    /* This comment workarounds a code generation problem */

    /* Gain: '<S5>/w1_scaling' */
    motor_io_position_B.w1_scaling = motor_io_position_P.w1_scaling_Gain *
      motor_io_position_B.SFunction2;

    /* Outputs for Triggered SubSystem: '<S5>/DS1104ENC_SET_POS_C1' incorporates:
     *  TriggerPort: '<S15>/Trigger'
     */
    if (rtmIsMajorTimeStep(motor_io_position_M)) {
      /* Constant: '<S5>/Reset enc' */
      zcEvent = rt_ZCFcn(RISING_ZERO_CROSSING,
                         &motor_io_position_PrevZCX.DS1104ENC_SET_POS_C1_Trig_ZCE,
                         (motor_io_position_P.Resetenc_Value));
      if (zcEvent != NO_ZCEVENT) {
        /* S-Function (rti_commonblock): '<S15>/S-Function1' */
        /* This comment workarounds a code generation problem */

        /* dSPACE I/O Board DS1104 Unit:ENC_SET */
        ds1104_inc_position_write(1, 0, DS1104_INC_LINE_SUBDIV_4);
      }
    }

    /* End of Outputs for SubSystem: '<S5>/DS1104ENC_SET_POS_C1' */

    /* S-Function (rti_commonblock): '<S14>/S-Function1' */
    /* This comment workarounds a code generation problem */

    /* S-Function (rti_commonblock): '<S14>/S-Function2' */
    /* This comment workarounds a code generation problem */
  }
}
/*
 * This function updates continuous states using the ODE3 fixed-step
 * solver algorithm
 */
static void rt_ertODEUpdateContinuousStates(RTWSolverInfo *si )
{
  /* Solver Matrices */
  static const real_T rt_ODE3_A[3] = {
    1.0/2.0, 3.0/4.0, 1.0
  };

  static const real_T rt_ODE3_B[3][3] = {
    { 1.0/2.0, 0.0, 0.0 },

    { 0.0, 3.0/4.0, 0.0 },

    { 2.0/9.0, 1.0/3.0, 4.0/9.0 }
  };

  time_T t = rtsiGetT(si);
  time_T tnew = rtsiGetSolverStopTime(si);
  time_T h = rtsiGetStepSize(si);
  real_T *x = rtsiGetContStates(si);
  ODE3_IntgData *id = (ODE3_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 hB[3];
  int_T i;
  int_T nXc = 4;
  rtsiSetSimTimeStep(si,MINOR_TIME_STEP);

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

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

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

  rtsiSetT(si, t + h*rt_ODE3_A[0]);
  rtsiSetdX(si, f1);
  trajectoryModel_step();
  trajectoryModel_derivatives();

  /* f(:,3) = feval(odefile, t + hA(2), y + f*hB(:,2), args(:)(*)); */
  for (i = 0; i <= 1; i++) {
    hB[i] = h * rt_ODE3_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_ODE3_A[1]);
  rtsiSetdX(si, f2);
  trajectoryModel_step();
  trajectoryModel_derivatives();

  /* tnew = t + hA(3);
     ynew = y + f*hB(:,3); */
  for (i = 0; i <= 2; i++) {
    hB[i] = h * rt_ODE3_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, tnew);
  rtsiSetSimTimeStep(si,MAJOR_TIME_STEP);
}
/* Model step function */
void trajectoryModel_step(void)
{
  /* local block i/o variables */
  real_T rtb_Sqrt;
  real_T rtb_Divide1;
  int8_T rtAction;
  real_T rtb_Divide;
  real_T rtb_Add1;
  if (rtmIsMajorTimeStep(trajectoryModel_M)) {
    /* set solver stop time */
    if (!(trajectoryModel_M->Timing.clockTick0+1)) {
      rtsiSetSolverStopTime(&trajectoryModel_M->solverInfo,
                            ((trajectoryModel_M->Timing.clockTickH0 + 1) *
        trajectoryModel_M->Timing.stepSize0 * 4294967296.0));
    } else {
      rtsiSetSolverStopTime(&trajectoryModel_M->solverInfo,
                            ((trajectoryModel_M->Timing.clockTick0 + 1) *
        trajectoryModel_M->Timing.stepSize0 +
        trajectoryModel_M->Timing.clockTickH0 *
        trajectoryModel_M->Timing.stepSize0 * 4294967296.0));
    }
  }                                    /* end MajorTimeStep */

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

  /* Integrator: '<Root>/x' */
  trajectoryModel_B.x = trajectoryModel_X.x_CSTATE;

  /* Sum: '<S3>/x-Planet_x' incorporates:
   *  Constant: '<Root>/0 '
   */
  rtb_Divide1 = trajectoryModel_B.x - trajectoryModel_P._Value;

  /* Integrator: '<Root>/y ' */
  trajectoryModel_B.y = trajectoryModel_X.y_CSTATE;

  /* Sqrt: '<S3>/Sqrt' incorporates:
   *  Product: '<S3>/(x-Planet_x)^2'
   *  Product: '<S3>/y^2'
   *  Sum: '<S3>/(x-Planet_x)^2+y^2'
   */
  rtb_Sqrt = sqrt(rtb_Divide1 * rtb_Divide1 + trajectoryModel_B.y *
                  trajectoryModel_B.y);

  /* If: '<Root>/If' incorporates:
   *  Constant: '<Root>/stopRadius'
   *  Constant: '<Root>/terminate'
   */
  if (rtmIsMajorTimeStep(trajectoryModel_M)) {
    if (rtb_Sqrt < trajectoryModel_P.stopRadius) {
      rtAction = 0;
    } else {
      rtAction = 1;
    }

    trajectoryModel_DW.If_ActiveSubsystem = rtAction;
  } else {
    rtAction = trajectoryModel_DW.If_ActiveSubsystem;
  }

  switch (rtAction) {
   case 0:
    /* Outputs for IfAction SubSystem: '<Root>/If Action Subsystem' incorporates:
     *  ActionPort: '<S1>/Action Port'
     */
    trajectoryMod_IfActionSubsystem(rtb_Sqrt,
      &trajectoryModel_B.IfActionSubsystem);

    /* End of Outputs for SubSystem: '<Root>/If Action Subsystem' */
    break;

   case 1:
    /* Outputs for IfAction SubSystem: '<Root>/If Action Subsystem1' incorporates:
     *  ActionPort: '<S2>/Action Port'
     */
    trajectoryMod_IfActionSubsystem(trajectoryModel_P.terminate_Value,
      &trajectoryModel_B.IfActionSubsystem1);

    /* End of Outputs for SubSystem: '<Root>/If Action Subsystem1' */
    break;
  }

  /* End of If: '<Root>/If' */
  if (rtmIsMajorTimeStep(trajectoryModel_M)) {
    /* Stop: '<Root>/Stop Simulation' */
    if (trajectoryModel_B.IfActionSubsystem1.In1 != 0.0) {
      rtmSetStopRequested(trajectoryModel_M, 1);
    }

    /* End of Stop: '<Root>/Stop Simulation' */
  }

  /* Integrator: '<Root>/dx' */
  trajectoryModel_B.x_dot = trajectoryModel_X.dx_CSTATE;

  /* Integrator: '<Root>/dy' */
  trajectoryModel_B.y_dot = trajectoryModel_X.dy_CSTATE;
  if (rtmIsMajorTimeStep(trajectoryModel_M)) {
  }

  /* Sum: '<S4>/x-Planet_x' incorporates:
   *  Constant: '<Root>/Earth_x'
   */
  rtb_Divide1 = trajectoryModel_B.x - (-trajectoryModel_P.mu);

  /* Sqrt: '<S4>/Sqrt' incorporates:
   *  Product: '<S4>/(x-Planet_x)^2'
   *  Product: '<S4>/y^2'
   *  Sum: '<S4>/(x-Planet_x)^2+y^2'
   */
  rtb_Divide1 = sqrt(rtb_Divide1 * rtb_Divide1 + trajectoryModel_B.y *
                     trajectoryModel_B.y);

  /* Product: '<Root>/Divide1' incorporates:
   *  Constant: '<Root>/Moon_x Earth mass'
   *  Product: '<Root>/Divide2'
   */
  rtb_Divide1 = (1.0 - trajectoryModel_P.mu) / (rtb_Divide1 * rtb_Divide1 *
    rtb_Divide1);

  /* Sum: '<S5>/x-Planet_x' incorporates:
   *  Constant: '<Root>/Moon_x Earth mass'
   */
  rtb_Divide = trajectoryModel_B.x - (1.0 - trajectoryModel_P.mu);

  /* Sqrt: '<S5>/Sqrt' incorporates:
   *  Product: '<S5>/(x-Planet_x)^2'
   *  Product: '<S5>/y^2'
   *  Sum: '<S5>/(x-Planet_x)^2+y^2'
   */
  rtb_Divide = sqrt(rtb_Divide * rtb_Divide + trajectoryModel_B.y *
                    trajectoryModel_B.y);

  /* Product: '<Root>/Divide' incorporates:
   *  Constant: '<Root>/Moon mass'
   *  Product: '<Root>/Divide3'
   */
  rtb_Divide = trajectoryModel_P.mu / (rtb_Divide * rtb_Divide * rtb_Divide);

  /* Sum: '<Root>/Add1' incorporates:
   *  Constant: '<Root>/2'
   */
  rtb_Add1 = (trajectoryModel_P._Value_f - rtb_Divide1) - rtb_Divide;

  /* Sum: '<Root>/Add' incorporates:
   *  Gain: '<Root>/Gain'
   *  Product: '<Root>/Product'
   */
  trajectoryModel_B.Add = trajectoryModel_B.y * rtb_Add1 -
    trajectoryModel_P.Gain_Gain * trajectoryModel_B.x_dot;

  /* Sum: '<Root>/Add6' incorporates:
   *  Constant: '<Root>/Earth_x'
   *  Constant: '<Root>/Moon_x Earth mass'
   *  Gain: '<Root>/Gain1'
   *  Product: '<Root>/Product5'
   *  Product: '<Root>/Product6'
   *  Product: '<Root>/Product7'
   *  Sum: '<Root>/Add7'
   */
  trajectoryModel_B.Add6 = (((1.0 - trajectoryModel_P.mu) * rtb_Divide +
    -trajectoryModel_P.mu * rtb_Divide1) + trajectoryModel_B.x * rtb_Add1) +
    trajectoryModel_P.Gain1_Gain * trajectoryModel_B.y_dot;
  if (rtmIsMajorTimeStep(trajectoryModel_M)) {
    /* Matfile logging */
    rt_UpdateTXYLogVars(trajectoryModel_M->rtwLogInfo,
                        (trajectoryModel_M->Timing.t));
  }                                    /* end MajorTimeStep */

  if (rtmIsMajorTimeStep(trajectoryModel_M)) {
    /* signal main to stop simulation */
    {                                  /* Sample time: [0.0s, 0.0s] */
      if ((rtmGetTFinal(trajectoryModel_M)!=-1) &&
          !((rtmGetTFinal(trajectoryModel_M)-
             (((trajectoryModel_M->Timing.clockTick1+
                trajectoryModel_M->Timing.clockTickH1* 4294967296.0)) * 0.01)) >
            (((trajectoryModel_M->Timing.clockTick1+
               trajectoryModel_M->Timing.clockTickH1* 4294967296.0)) * 0.01) *
            (DBL_EPSILON))) {
        rtmSetErrorStatus(trajectoryModel_M, "Simulation finished");
      }
    }

    rt_ertODEUpdateContinuousStates(&trajectoryModel_M->solverInfo);

    /* Update absolute time for base rate */
    /* The "clockTick0" counts the number of times the code of this task has
     * been executed. The absolute time is the multiplication of "clockTick0"
     * and "Timing.stepSize0". Size of "clockTick0" ensures timer will not
     * overflow during the application lifespan selected.
     * Timer of this task consists of two 32 bit unsigned integers.
     * The two integers represent the low bits Timing.clockTick0 and the high bits
     * Timing.clockTickH0. When the low bit overflows to 0, the high bits increment.
     */
    if (!(++trajectoryModel_M->Timing.clockTick0)) {
      ++trajectoryModel_M->Timing.clockTickH0;
    }

    trajectoryModel_M->Timing.t[0] = rtsiGetSolverStopTime
      (&trajectoryModel_M->solverInfo);

    {
      /* Update absolute timer for sample time: [0.01s, 0.0s] */
      /* The "clockTick1" counts the number of times the code of this task has
       * been executed. The resolution of this integer timer is 0.01, which is the step size
       * of the task. Size of "clockTick1" ensures timer will not overflow during the
       * application lifespan selected.
       * Timer of this task consists of two 32 bit unsigned integers.
       * The two integers represent the low bits Timing.clockTick1 and the high bits
       * Timing.clockTickH1. When the low bit overflows to 0, the high bits increment.
       */
      trajectoryModel_M->Timing.clockTick1++;
      if (!trajectoryModel_M->Timing.clockTick1) {
        trajectoryModel_M->Timing.clockTickH1++;
      }
    }
  }                                    /* end MajorTimeStep */
}
예제 #8
0
/* Model step function */
void Position_TiltModelClass::step()
{
  real_T rtb_Filter;
  if (rtmIsMajorTimeStep((&Position_Tilt_M))) {
    /* set solver stop time */
    if (!((&Position_Tilt_M)->Timing.clockTick0+1)) {
      rtsiSetSolverStopTime(&(&Position_Tilt_M)->solverInfo, (((&Position_Tilt_M)
        ->Timing.clockTickH0 + 1) * (&Position_Tilt_M)->Timing.stepSize0 *
        4294967296.0));
    } else {
      rtsiSetSolverStopTime(&(&Position_Tilt_M)->solverInfo, (((&Position_Tilt_M)
        ->Timing.clockTick0 + 1) * (&Position_Tilt_M)->Timing.stepSize0 +
        (&Position_Tilt_M)->Timing.clockTickH0 * (&Position_Tilt_M)
        ->Timing.stepSize0 * 4294967296.0));
    }
  }                                    /* end MajorTimeStep */

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

  /* Sum: '<S1>/Sum' incorporates:
   *  Inport: '<Root>/Pos'
   *  Inport: '<Root>/PosRef'
   */
  rtb_Filter = Position_Tilt_U.PosRef[0] - Position_Tilt_U.Pos[0];

  /* Gain: '<S2>/Filter Coefficient' incorporates:
   *  Gain: '<S2>/Derivative Gain'
   *  Integrator: '<S2>/Filter'
   *  Sum: '<S2>/SumD'
   */
  Position_Tilt_B.FilterCoefficient = (Position_Tilt_P.Kd_N * rtb_Filter -
    Position_Tilt_X.Filter_CSTATE) * Position_Tilt_P.PIDController_N;

  /* Outport: '<Root>/u_des' incorporates:
   *  Gain: '<S2>/Proportional Gain'
   *  Sum: '<S2>/Sum'
   */
  Position_Tilt_Y.u_des = Position_Tilt_P.Kp_N * rtb_Filter +
    Position_Tilt_B.FilterCoefficient;

  /* Sum: '<S1>/Sum1' incorporates:
   *  Inport: '<Root>/Pos'
   *  Inport: '<Root>/PosRef'
   */
  rtb_Filter = Position_Tilt_U.PosRef[1] - Position_Tilt_U.Pos[1];

  /* Gain: '<S3>/Filter Coefficient' incorporates:
   *  Gain: '<S3>/Derivative Gain'
   *  Integrator: '<S3>/Filter'
   *  Sum: '<S3>/SumD'
   */
  Position_Tilt_B.FilterCoefficient_f = (Position_Tilt_P.Kd_E * rtb_Filter -
    Position_Tilt_X.Filter_CSTATE_b) * Position_Tilt_P.PIDController1_N;

  /* Outport: '<Root>/v_des' incorporates:
   *  Gain: '<S3>/Proportional Gain'
   *  Sum: '<S3>/Sum'
   */
  Position_Tilt_Y.v_des = Position_Tilt_P.Kp_E * rtb_Filter +
    Position_Tilt_B.FilterCoefficient_f;
  if (rtmIsMajorTimeStep((&Position_Tilt_M))) {
    rt_ertODEUpdateContinuousStates(&(&Position_Tilt_M)->solverInfo);

    /* Update absolute time for base rate */
    /* The "clockTick0" counts the number of times the code of this task has
     * been executed. The absolute time is the multiplication of "clockTick0"
     * and "Timing.stepSize0". Size of "clockTick0" ensures timer will not
     * overflow during the application lifespan selected.
     * Timer of this task consists of two 32 bit unsigned integers.
     * The two integers represent the low bits Timing.clockTick0 and the high bits
     * Timing.clockTickH0. When the low bit overflows to 0, the high bits increment.
     */
    if (!(++(&Position_Tilt_M)->Timing.clockTick0)) {
      ++(&Position_Tilt_M)->Timing.clockTickH0;
    }

    (&Position_Tilt_M)->Timing.t[0] = rtsiGetSolverStopTime(&(&Position_Tilt_M
      )->solverInfo);

    {
      /* Update absolute timer for sample time: [0.01s, 0.0s] */
      /* The "clockTick1" counts the number of times the code of this task has
       * been executed. The resolution of this integer timer is 0.01, which is the step size
       * of the task. Size of "clockTick1" ensures timer will not overflow during the
       * application lifespan selected.
       * Timer of this task consists of two 32 bit unsigned integers.
       * The two integers represent the low bits Timing.clockTick1 and the high bits
       * Timing.clockTickH1. When the low bit overflows to 0, the high bits increment.
       */
      (&Position_Tilt_M)->Timing.clockTick1++;
      if (!(&Position_Tilt_M)->Timing.clockTick1) {
        (&Position_Tilt_M)->Timing.clockTickH1++;
      }
    }
  }                                    /* end MajorTimeStep */
}
예제 #9
0
/* This function updates continuous states using the ODE3 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);
  ODE3_IntgData *id = (ODE3_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 hB[3];
  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);
  Mechanics_derivatives();

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

  rtsiSetT(si, t + h*rt_ODE3_A[0]);
  rtsiSetdX(si, f1);
  Mechanics_output(0);
  Mechanics_derivatives();

  /* f(:,3) = feval(odefile, t + hA(2), y + f*hB(:,2), args(:)(*)); */
  for (i = 0; i <= 1; i++)
    hB[i] = h * rt_ODE3_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_ODE3_A[1]);
  rtsiSetdX(si, f2);
  Mechanics_output(0);
  Mechanics_derivatives();

  /* tnew = t + hA(3);
     ynew = y + f*hB(:,3); */
  for (i = 0; i <= 2; i++)
    hB[i] = h * rt_ODE3_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, tnew);
  Mechanics_output(0);
  Mechanics_projection();
  rtsiSetSimTimeStep(si,MAJOR_TIME_STEP);
}
예제 #10
0
/* Model output function */
void Mechanics_output(int_T tid)
{
  /* Update absolute time of base rate at minor time step */
  if (rtmIsMinorTimeStep(Mechanics_M)) {
    Mechanics_M->Timing.t[0] = rtsiGetT(&Mechanics_M->solverInfo);
  }

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

  if (rtmIsMajorTimeStep(Mechanics_M) &&
      Mechanics_M->Timing.TaskCounters.TID[1] == 0) {
    /* Level2 S-Function Block: '<Root>/Arduino' (QueryInstrument) */
    {
      SimStruct *rts = Mechanics_M->childSfunctions[0];
      sfcnOutputs(rts, 1);
    }

    /* Gain: '<Root>/Gain' */
    Mechanics_B.Gain = Mechanics_P.Gain_Gain * Mechanics_B.Arduino;
  }

  /* S-Function Block: <S6>/Block#1 */
  {
    _rtMech_PWORK *mechWork = (_rtMech_PWORK *) Mechanics_DWork.Block1_PWORK;
    mechWork->genSimData.time = Mechanics_M->Timing.t[0];
    mechWork->outSimData.majorTimestep = rtmIsMajorTimeStep(Mechanics_M);
    if (kinematicSfcnOutputMethod(mechWork->mechanism, &(mechWork->genSimData),
         &(mechWork->outSimData))) {
      {
        const ErrorRecord *err = getErrorMsg();
        static char_T errorMsg[1024];
        sprintf(errorMsg,
                err->errorMsg,
                err->blocks[0],
                err->blocks[1],
                err->blocks[2],
                err->blocks[3],
                err->blocks[4]);
        rtmSetErrorStatus(Mechanics_M, errorMsg);
        return;
      }
    }
  }

  if (rtmIsMajorTimeStep(Mechanics_M) &&
      Mechanics_M->Timing.TaskCounters.TID[1] == 0) {
    /* Gain: '<S6>/_gravity_conversion' incorporates:
     *  Constant: '<S5>/SOURCE_BLOCK'
     */
    Mechanics_B._gravity_conversion[0] = Mechanics_P._gravity_conversion_Gain *
      Mechanics_P.SOURCE_BLOCK_Value[0];
    Mechanics_B._gravity_conversion[1] = Mechanics_P._gravity_conversion_Gain *
      Mechanics_P.SOURCE_BLOCK_Value[1];
    Mechanics_B._gravity_conversion[2] = Mechanics_P._gravity_conversion_Gain *
      Mechanics_P.SOURCE_BLOCK_Value[2];
  }

  /* S-Function Block: <S6>/Block#2 */
  {
    _rtMech_PWORK *mechWork = (_rtMech_PWORK *) Mechanics_DWork.Block2_PWORK;
    mechWork->genSimData.time = Mechanics_M->Timing.t[0];
    mechWork->outSimData.majorTimestep = rtmIsMajorTimeStep(Mechanics_M);
    if (dynamicSfcnOutputMethod(mechWork->mechanism, &(mechWork->genSimData),
         &(mechWork->outSimData))) {
      {
        const ErrorRecord *err = getErrorMsg();
        static char_T errorMsg[1024];
        sprintf(errorMsg,
                err->errorMsg,
                err->blocks[0],
                err->blocks[1],
                err->blocks[2],
                err->blocks[3],
                err->blocks[4]);
        rtmSetErrorStatus(Mechanics_M, errorMsg);
        return;
      }
    }
  }

  /* S-Function Block: <S6>/Block#3 */
  {
    _rtMech_PWORK *mechWork = (_rtMech_PWORK *) Mechanics_DWork.Block3_PWORK;
    mechWork->genSimData.time = Mechanics_M->Timing.t[0];
    mechWork->outSimData.majorTimestep = rtmIsMajorTimeStep(Mechanics_M);
    if (eventSfcnOutputMethod(mechWork->mechanism, &(mechWork->genSimData),
         &(mechWork->outSimData))) {
      {
        const ErrorRecord *err = getErrorMsg();
        static char_T errorMsg[1024];
        sprintf(errorMsg,
                err->errorMsg,
                err->blocks[0],
                err->blocks[1],
                err->blocks[2],
                err->blocks[3],
                err->blocks[4]);
        rtmSetErrorStatus(Mechanics_M, errorMsg);
        return;
      }
    }
  }

  UNUSED_PARAMETER(tid);
}
예제 #11
0
void rt_ODEUpdateContinuousStates(RTWSolverInfo *si)
{
    time_T    t          = rtsiGetT(si);
    time_T    tnew       = rtsiGetSolverStopTime(si);
    time_T    h          = rtsiGetStepSize(si);
    real_T    *x         = rtsiGetContStates(si);
    IntgData  *id        = 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    hB[3];
    int_T     i;

#ifdef NCSTATES
    int_T     nXc        = NCSTATES;
#else
    int_T     nXc        = rtsiGetNumContStates(si);
#endif

    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);
    DERIVATIVES(si);

    /* f(:,2) = feval(odefile, t + hA(1), y + f*hB(:,1), args(:)(*)); */
    hB[0] = h * rt_ODE3_B[0][0];
    for (i = 0; i < nXc; i++) {
	x[i] = y[i] + (f0[i]*hB[0]);
    }
    rtsiSetT(si, t + h*rt_ODE3_A[0]);
    rtsiSetdX(si, f1);
    OUTPUTS(si,0);
    DERIVATIVES(si);

    /* f(:,3) = feval(odefile, t + hA(2), y + f*hB(:,2), args(:)(*)); */
    for (i = 0; i <= 1; i++) hB[i] = h * rt_ODE3_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_ODE3_A[1]);
    rtsiSetdX(si, f2);
    OUTPUTS(si,0);
    DERIVATIVES(si);

    /* tnew = t + hA(3);
       ynew = y + f*hB(:,3); */
    for (i = 0; i <= 2; i++) hB[i] = h * rt_ODE3_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, tnew);

    PROJECTION(si);

    rtsiSetSimTimeStep(si,MAJOR_TIME_STEP);
}
예제 #12
0
파일: motor_io.c 프로젝트: AndFroSwe/MF2007
/* Model output function */
void motor_io_output(void)
{
  ZCEventType zcEvent;
  real_T u0;
  real_T u1;
  real_T u2;
  if (rtmIsMajorTimeStep(motor_io_M)) {
    /* set solver stop time */
    if (!(motor_io_M->Timing.clockTick0+1)) {
      rtsiSetSolverStopTime(&motor_io_M->solverInfo,
                            ((motor_io_M->Timing.clockTickH0 + 1) *
        motor_io_M->Timing.stepSize0 * 4294967296.0));
    } else {
      rtsiSetSolverStopTime(&motor_io_M->solverInfo,
                            ((motor_io_M->Timing.clockTick0 + 1) *
        motor_io_M->Timing.stepSize0 + motor_io_M->Timing.clockTickH0 *
        motor_io_M->Timing.stepSize0 * 4294967296.0));
    }
  }                                    /* end MajorTimeStep */

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

  if (rtmIsMajorTimeStep(motor_io_M)) {
    /* S-Function (rti_commonblock): '<S14>/S-Function1' */
    /* This comment workarounds a code generation problem */

    /* Gain: '<S6>/fi1_scaling' */
    motor_io_B.fi1_scaling = motor_io_P.fi1_scaling_Gain * motor_io_B.SFunction1;

    /* MATLAB Function: '<Root>/Traject_and_Model_function 1' incorporates:
     *  Constant: '<Root>/RS'
     *  Constant: '<Root>/Reset enc'
     */
    mot_Traject_and_Model_function1(motor_io_P.Resetenc_Value,
      motor_io_P.RS_Value, motor_io_B.fi1_scaling,
      &motor_io_B.sf_Traject_and_Model_function1,
      &motor_io_DW.sf_Traject_and_Model_function1);

    /* Gain: '<Root>/Gain' */
    motor_io_B.Gain = motor_io_P.Gain_Gain *
      motor_io_B.sf_Traject_and_Model_function1.volt;

    /* Saturate: '<S2>/Saturation' */
    u0 = motor_io_B.Gain;
    u1 = motor_io_P.Saturation_LowerSat;
    u2 = motor_io_P.Saturation_UpperSat;
    if (u0 > u2) {
      motor_io_B.Volt = u2;
    } else if (u0 < u1) {
      motor_io_B.Volt = u1;
    } else {
      motor_io_B.Volt = u0;
    }

    /* End of Saturate: '<S2>/Saturation' */

    /* Gain: '<S2>/pwm_skalning' */
    motor_io_B.pwm_skalning = motor_io_P.pwm_skalning_Gain * motor_io_B.Volt;

    /* Sum: '<S2>/Sum' incorporates:
     *  Constant: '<S2>/pwm_offstet'
     */
    motor_io_B.Sum = motor_io_B.pwm_skalning + motor_io_P.pwm_offstet_Value;

    /* S-Function (rti_commonblock): '<S10>/S-Function1' */
    /* This comment workarounds a code generation problem */

    /* dSPACE I/O Board DS1104 #1 Unit:PWM Group:PWM */
    ds1104_slave_dsp_pwm_duty_write(0, rti_slv1104_fcn_index[6], motor_io_B.Sum);

    /* S-Function (rti_commonblock): '<S10>/S-Function2' */
    /* This comment workarounds a code generation problem */

    /* S-Function (rti_commonblock): '<S10>/S-Function3' */
    /* This comment workarounds a code generation problem */

    /* S-Function (rti_commonblock): '<S10>/S-Function4' */
    /* This comment workarounds a code generation problem */

    /* DataTypeConversion: '<S2>/Data Type Conversion' incorporates:
     *  Constant: '<S2>/Enable[1_Off, 0_On]'
     */
    motor_io_B.DataTypeConversion = (motor_io_P.Enable1_Off0_On_Value != 0.0);

    /* S-Function (rti_commonblock): '<S9>/S-Function1' */
    /* This comment workarounds a code generation problem */

    /* dSPACE I/O Board DS1104 #1 Unit:BIT_IO Group:BIT_OUT */
    if (motor_io_B.DataTypeConversion > 0) {
      ds1104_bit_io_set(DS1104_DIO0);
    } else {
      ds1104_bit_io_clear(DS1104_DIO0);
    }
  }

  /* Integrator: '<S1>/Integrator1' */
  motor_io_B.Integrator1 = motor_io_X.Integrator1_CSTATE;
  if (rtmIsMajorTimeStep(motor_io_M)) {
    /* MATLAB Function: '<Root>/Traject_and_Model_function 3' incorporates:
     *  Constant: '<Root>/RS'
     *  Constant: '<Root>/Reset enc'
     */
    mot_Traject_and_Model_function1(motor_io_P.Resetenc_Value,
      motor_io_P.RS_Value, motor_io_B.Integrator1,
      &motor_io_B.sf_Traject_and_Model_function3,
      &motor_io_DW.sf_Traject_and_Model_function3);
  }

  /* Integrator: '<S1>/Integrator' */
  motor_io_B.Integrator = motor_io_X.Integrator_CSTATE;

  /* Gain: '<S1>/Gain1' */
  motor_io_B.Gain1 = motor_io_P.Gain1_Gain * motor_io_B.Integrator;

  /* Sum: '<S1>/Add' */
  motor_io_B.Add = motor_io_B.sf_Traject_and_Model_function3.volt -
    motor_io_B.Gain1;

  /* Gain: '<S1>/k//R ' */
  motor_io_B.kR = motor_io_P.kR_Gain * motor_io_B.Add;

  /* Saturate: '<S7>/Saturate to Fc' */
  u0 = motor_io_B.kR;
  u1 = motor_io_P.SaturatetoFc_LowerSat;
  u2 = motor_io_P.F_c_upper;
  if (u0 > u2) {
    motor_io_B.Stickslipregion = u2;
  } else if (u0 < u1) {
    motor_io_B.Stickslipregion = u1;
  } else {
    motor_io_B.Stickslipregion = u0;
  }

  /* End of Saturate: '<S7>/Saturate to Fc' */

  /* Abs: '<S7>/Abs' */
  motor_io_B.Abs = fabs(motor_io_B.Integrator);

  /* RelationalOperator: '<S8>/Compare' incorporates:
   *  Constant: '<S8>/Constant'
   */
  motor_io_B.Compare = (motor_io_B.Abs <= motor_io_P.Constant_Value);

  /* Gain: '<S7>/Vicous friction' */
  motor_io_B.Vicousfriction = motor_io_P.Vicousfriction_Gain *
    motor_io_B.Integrator;

  /* Signum: '<S7>/Sign' */
  u0 = motor_io_B.Integrator;
  if (u0 < 0.0) {
    motor_io_B.Sign = -1.0;
  } else if (u0 > 0.0) {
    motor_io_B.Sign = 1.0;
  } else if (u0 == 0.0) {
    motor_io_B.Sign = 0.0;
  } else {
    motor_io_B.Sign = u0;
  }

  /* End of Signum: '<S7>/Sign' */

  /* Product: '<S7>/Product' incorporates:
   *  Constant: '<S7>/F_c'
   */
  motor_io_B.Product = motor_io_P.F_c * motor_io_B.Sign;

  /* Sum: '<S7>/Add' */
  motor_io_B.Viscousregion = motor_io_B.Vicousfriction + motor_io_B.Product;

  /* Switch: '<S7>/Switch' */
  if (motor_io_B.Compare) {
    motor_io_B.Friction = motor_io_B.Stickslipregion;
  } else {
    motor_io_B.Friction = motor_io_B.Viscousregion;
  }

  /* End of Switch: '<S7>/Switch' */

  /* Sum: '<S1>/Add1' */
  motor_io_B.Add1 = motor_io_B.kR - motor_io_B.Friction;
  if (rtmIsMajorTimeStep(motor_io_M)) {
    /* Gain: '<S1>/Gain2' incorporates:
     *  Constant: '<S1>/Load inertia'
     */
    motor_io_B.Gain2 = motor_io_P.Gain2_Gain * motor_io_P.J1;

    /* Sum: '<S1>/Add2' incorporates:
     *  Constant: '<S1>/Motor inertia'
     */
    motor_io_B.Add2 = motor_io_B.Gain2 + motor_io_P.Motorinertia_Value;
  }

  /* Product: '<S1>/Inertias 1//J' */
  motor_io_B.Inertias1J = 1.0 / motor_io_B.Add2 * motor_io_B.Add1;
  if (rtmIsMajorTimeStep(motor_io_M)) {
    /* S-Function (rti_commonblock): '<S14>/S-Function2' */
    /* This comment workarounds a code generation problem */

    /* Gain: '<S6>/w1_scaling' */
    motor_io_B.w1_scaling = motor_io_P.w1_scaling_Gain * motor_io_B.SFunction2;

    /* Outputs for Triggered SubSystem: '<S6>/DS1104ENC_SET_POS_C1' incorporates:
     *  TriggerPort: '<S16>/Trigger'
     */
    if (rtmIsMajorTimeStep(motor_io_M)) {
      /* Constant: '<S6>/Reset enc' */
      zcEvent = rt_ZCFcn(RISING_ZERO_CROSSING,
                         &motor_io_PrevZCX.DS1104ENC_SET_POS_C1_Trig_ZCE,
                         (motor_io_P.Resetenc_Value_k));
      if (zcEvent != NO_ZCEVENT) {
        /* S-Function (rti_commonblock): '<S16>/S-Function1' */
        /* This comment workarounds a code generation problem */

        /* dSPACE I/O Board DS1104 Unit:ENC_SET */
        ds1104_inc_position_write(1, 0, DS1104_INC_LINE_SUBDIV_4);
      }
    }

    /* End of Outputs for SubSystem: '<S6>/DS1104ENC_SET_POS_C1' */

    /* S-Function (rti_commonblock): '<S15>/S-Function1' */
    /* This comment workarounds a code generation problem */

    /* S-Function (rti_commonblock): '<S15>/S-Function2' */
    /* This comment workarounds a code generation problem */
  }

  /* SignalGenerator: '<Root>/SinGenerator' */
  motor_io_B.SinGenerator = sin(motor_io_P.SinGenerator_Frequency *
    motor_io_M->Timing.t[0]) * motor_io_P.SinGenerator_Amplitude;

  /* SignalGenerator: '<Root>/SquareGenerator' */
  u0 = motor_io_P.SquareGenerator_Frequency * motor_io_M->Timing.t[0];
  if (u0 - floor(u0) >= 0.5) {
    motor_io_B.SquareGenerator = motor_io_P.SquareGenerator_Amplitude;
  } else {
    motor_io_B.SquareGenerator = -motor_io_P.SquareGenerator_Amplitude;
  }

  /* End of SignalGenerator: '<Root>/SquareGenerator' */

  /* Switch: '<Root>/Switch' incorporates:
   *  Constant: '<Root>/SignalSelector[0Square,1Sine]'
   */
  if (motor_io_P.SignalSelector0Square1Sine_Valu != 0.0) {
    motor_io_B.Switch = motor_io_B.SinGenerator;
  } else {
    motor_io_B.Switch = motor_io_B.SquareGenerator;
  }

  /* End of Switch: '<Root>/Switch' */
}
예제 #13
0
/* Model step function */
void AttitudeModelClass::step()
{
  real_T Sphi;
  real_T Cphi;
  real_T Ctheta;
  real_T rtb_Sum4;
  real_T rtb_ProportionalGain;
  real_T rtb_ProportionalGain_h;
  real_T rtb_Rates_B[3];
  real_T rtb_Sum6;
  real_T rtb_Saturate_l;
  real_T rtb_Sum_k;
  real_T tmp[9];
  int32_T i;
  if (rtmIsMajorTimeStep((&Attitude_M))) {
    /* set solver stop time */
    if (!((&Attitude_M)->Timing.clockTick0+1)) {
      rtsiSetSolverStopTime(&(&Attitude_M)->solverInfo, (((&Attitude_M)
        ->Timing.clockTickH0 + 1) * (&Attitude_M)->Timing.stepSize0 *
        4294967296.0));
    } else {
      rtsiSetSolverStopTime(&(&Attitude_M)->solverInfo, (((&Attitude_M)
        ->Timing.clockTick0 + 1) * (&Attitude_M)->Timing.stepSize0 +
        (&Attitude_M)->Timing.clockTickH0 * (&Attitude_M)->Timing.stepSize0 *
        4294967296.0));
    }
  }                                    /* end MajorTimeStep */

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

  /* Saturate: '<S1>/Saturation' incorporates:
   *  Inport: '<Root>/Stick'
   */
  if (Attitude_U.Stick[0] > Attitude_P.Saturation_UpperSat) {
    rtb_Sum4 = Attitude_P.Saturation_UpperSat;
  } else if (Attitude_U.Stick[0] < Attitude_P.Saturation_LowerSat) {
    rtb_Sum4 = Attitude_P.Saturation_LowerSat;
  } else {
    rtb_Sum4 = Attitude_U.Stick[0];
  }

  /* Sum: '<S1>/Sum' incorporates:
   *  Gain: '<S1>/Yaw-rate1'
   *  Inport: '<Root>/IMU_Attitude'
   *  Saturate: '<S1>/Saturation'
   */
  rtb_Sum4 = Attitude_P.rollMax * rtb_Sum4 - Attitude_U.IMU_Attitude[0];

  /* Gain: '<S3>/Proportional Gain' */
  rtb_ProportionalGain = Attitude_P.KRP * rtb_Sum4;

  /* Gain: '<S3>/Filter Coefficient' incorporates:
   *  Gain: '<S3>/Derivative Gain'
   *  Integrator: '<S3>/Filter'
   *  Sum: '<S3>/SumD'
   */
  Attitude_B.FilterCoefficient = (Attitude_P.KRD * rtb_Sum4 -
    Attitude_X.Filter_CSTATE) * Attitude_P.N;

  /* Saturate: '<S1>/Saturation1' incorporates:
   *  Inport: '<Root>/Stick'
   */
  if (Attitude_U.Stick[1] > Attitude_P.Saturation1_UpperSat) {
    rtb_Sum4 = Attitude_P.Saturation1_UpperSat;
  } else if (Attitude_U.Stick[1] < Attitude_P.Saturation1_LowerSat) {
    rtb_Sum4 = Attitude_P.Saturation1_LowerSat;
  } else {
    rtb_Sum4 = Attitude_U.Stick[1];
  }

  /* Sum: '<S1>/Sum1' incorporates:
   *  Gain: '<S1>/Yaw-rate2'
   *  Inport: '<Root>/IMU_Attitude'
   *  Saturate: '<S1>/Saturation1'
   */
  rtb_Sum4 = Attitude_P.pitchMax * rtb_Sum4 - Attitude_U.IMU_Attitude[1];

  /* Gain: '<S2>/Proportional Gain' */
  rtb_ProportionalGain_h = Attitude_P.KPP * rtb_Sum4;

  /* Gain: '<S2>/Filter Coefficient' incorporates:
   *  Gain: '<S2>/Derivative Gain'
   *  Integrator: '<S2>/Filter'
   *  Sum: '<S2>/SumD'
   */
  Attitude_B.FilterCoefficient_e = (Attitude_P.KPD * rtb_Sum4 -
    Attitude_X.Filter_CSTATE_m) * Attitude_P.N;

  /* Sum: '<S1>/Sum2' incorporates:
   *  Inport: '<Root>/IMU_Attitude'
   *  Inport: '<Root>/Stick'
   */
  rtb_Sum4 = Attitude_U.Stick[3] - Attitude_U.IMU_Attitude[2];

  /* Gain: '<S4>/Filter Coefficient' incorporates:
   *  Gain: '<S4>/Derivative Gain'
   *  Integrator: '<S4>/Filter'
   *  Sum: '<S4>/SumD'
   */
  Attitude_B.FilterCoefficient_d = (Attitude_P.KYD * rtb_Sum4 -
    Attitude_X.Filter_CSTATE_mi) * Attitude_P.N;

  /* Switch: '<S1>/Switch' incorporates:
   *  Gain: '<S1>/Yaw-rate3'
   *  Gain: '<S4>/Proportional Gain'
   *  Inport: '<Root>/Selector'
   *  Inport: '<Root>/Stick'
   *  Saturate: '<S1>/Saturation2'
   *  Sum: '<S4>/Sum'
   */
  if (Attitude_U.Selector >= Attitude_P.Switch_Threshold) {
    rtb_Sum4 = Attitude_P.KYP * rtb_Sum4 + Attitude_B.FilterCoefficient_d;
  } else {
    if (Attitude_U.Stick[2] > Attitude_P.Saturation2_UpperSat) {
      /* Saturate: '<S1>/Saturation2' */
      rtb_Sum4 = Attitude_P.Saturation2_UpperSat;
    } else if (Attitude_U.Stick[2] < Attitude_P.Saturation2_LowerSat) {
      /* Saturate: '<S1>/Saturation2' */
      rtb_Sum4 = Attitude_P.Saturation2_LowerSat;
    } else {
      /* Saturate: '<S1>/Saturation2' incorporates:
       *  Inport: '<Root>/Stick'
       */
      rtb_Sum4 = Attitude_U.Stick[2];
    }

    rtb_Sum4 *= Attitude_P.yawRateMax;
  }

  /* End of Switch: '<S1>/Switch' */

  /* MATLAB Function: '<S1>/To body from Earth_rates' incorporates:
   *  Inport: '<Root>/IMU_Attitude'
   */
  /* MATLAB Function 'Attitude Controller/To body from Earth_rates': '<S8>:1' */
  /* '<S8>:1:3' */
  /* '<S8>:1:4' */
  /* '<S8>:1:6' */
  Sphi = sin(Attitude_U.IMU_Attitude[0]);

  /* '<S8>:1:7' */
  Cphi = cos(Attitude_U.IMU_Attitude[0]);

  /* '<S8>:1:8' */
  /* '<S8>:1:9' */
  Ctheta = cos(Attitude_U.IMU_Attitude[1]);

  /* '<S8>:1:11' */
  /* '<S8>:1:15' */
  tmp[0] = 1.0;
  tmp[3] = 0.0;
  tmp[6] = -sin(Attitude_U.IMU_Attitude[1]);
  tmp[1] = 0.0;
  tmp[4] = Cphi;
  tmp[7] = Sphi * Ctheta;
  tmp[2] = 0.0;
  tmp[5] = -Sphi;
  tmp[8] = Cphi * Ctheta;

  /* SignalConversion: '<S8>/TmpSignal ConversionAt SFunction Inport2' incorporates:
   *  MATLAB Function: '<S1>/To body from Earth_rates'
   *  Sum: '<S2>/Sum'
   *  Sum: '<S3>/Sum'
   */
  rtb_ProportionalGain += Attitude_B.FilterCoefficient;
  rtb_ProportionalGain_h += Attitude_B.FilterCoefficient_e;

  /* MATLAB Function: '<S1>/To body from Earth_rates' incorporates:
   *  SignalConversion: '<S8>/TmpSignal ConversionAt SFunction Inport2'
   */
  for (i = 0; i < 3; i++) {
    rtb_Rates_B[i] = tmp[i + 6] * rtb_Sum4 + (tmp[i + 3] *
      rtb_ProportionalGain_h + tmp[i] * rtb_ProportionalGain);
  }

  /* Sum: '<S1>/Sum4' incorporates:
   *  Inport: '<Root>/IMU_Rates'
   */
  rtb_Sum4 = rtb_Rates_B[0] - Attitude_U.IMU_Rates[0];

  /* Gain: '<S5>/Filter Coefficient' incorporates:
   *  Gain: '<S5>/Derivative Gain'
   *  Integrator: '<S5>/Filter'
   *  Sum: '<S5>/SumD'
   */
  Attitude_B.FilterCoefficient_o = (Attitude_P.Kdp * rtb_Sum4 -
    Attitude_X.Filter_CSTATE_k) * Attitude_P.N;

  /* Sum: '<S5>/Sum' incorporates:
   *  Gain: '<S5>/Proportional Gain'
   *  Integrator: '<S5>/Integrator'
   */
  rtb_ProportionalGain = (Attitude_P.Kpp * rtb_Sum4 +
    Attitude_X.Integrator_CSTATE) + Attitude_B.FilterCoefficient_o;

  /* Saturate: '<S5>/Saturate' */
  if (rtb_ProportionalGain > Attitude_P.satp) {
    rtb_ProportionalGain_h = Attitude_P.satp;
  } else if (rtb_ProportionalGain < -Attitude_P.satp) {
    rtb_ProportionalGain_h = -Attitude_P.satp;
  } else {
    rtb_ProportionalGain_h = rtb_ProportionalGain;
  }

  /* End of Saturate: '<S5>/Saturate' */

  /* Sum: '<S1>/Sum5' incorporates:
   *  Inport: '<Root>/IMU_Rates'
   */
  Sphi = rtb_Rates_B[1] - Attitude_U.IMU_Rates[1];

  /* Gain: '<S6>/Filter Coefficient' incorporates:
   *  Gain: '<S6>/Derivative Gain'
   *  Integrator: '<S6>/Filter'
   *  Sum: '<S6>/SumD'
   */
  Attitude_B.FilterCoefficient_b = (Attitude_P.Kdq * Sphi -
    Attitude_X.Filter_CSTATE_e) * Attitude_P.N;

  /* Sum: '<S6>/Sum' incorporates:
   *  Gain: '<S6>/Proportional Gain'
   *  Integrator: '<S6>/Integrator'
   */
  Cphi = (Attitude_P.Kpq * Sphi + Attitude_X.Integrator_CSTATE_f) +
    Attitude_B.FilterCoefficient_b;

  /* Saturate: '<S6>/Saturate' */
  if (Cphi > Attitude_P.satq) {
    Ctheta = Attitude_P.satq;
  } else if (Cphi < -Attitude_P.satq) {
    Ctheta = -Attitude_P.satq;
  } else {
    Ctheta = Cphi;
  }

  /* End of Saturate: '<S6>/Saturate' */

  /* Sum: '<S1>/Sum6' incorporates:
   *  Inport: '<Root>/IMU_Rates'
   */
  rtb_Sum6 = rtb_Rates_B[2] - Attitude_U.IMU_Rates[2];

  /* Gain: '<S7>/Filter Coefficient' incorporates:
   *  Gain: '<S7>/Derivative Gain'
   *  Integrator: '<S7>/Filter'
   *  Sum: '<S7>/SumD'
   */
  Attitude_B.FilterCoefficient_oo = (Attitude_P.Kdr * rtb_Sum6 -
    Attitude_X.Filter_CSTATE_g) * Attitude_P.N;

  /* Sum: '<S7>/Sum' incorporates:
   *  Gain: '<S7>/Proportional Gain'
   *  Integrator: '<S7>/Integrator'
   */
  rtb_Sum_k = (Attitude_P.Kpr * rtb_Sum6 + Attitude_X.Integrator_CSTATE_h) +
    Attitude_B.FilterCoefficient_oo;

  /* Saturate: '<S7>/Saturate' */
  if (rtb_Sum_k > Attitude_P.satr) {
    rtb_Saturate_l = Attitude_P.satr;
  } else if (rtb_Sum_k < -Attitude_P.satr) {
    rtb_Saturate_l = -Attitude_P.satr;
  } else {
    rtb_Saturate_l = rtb_Sum_k;
  }

  /* End of Saturate: '<S7>/Saturate' */

  /* Outport: '<Root>/Moments' */
  Attitude_Y.Moments[0] = rtb_ProportionalGain_h;
  Attitude_Y.Moments[1] = Ctheta;
  Attitude_Y.Moments[2] = rtb_Saturate_l;

  /* Sum: '<S5>/SumI1' incorporates:
   *  Gain: '<S5>/Integral Gain'
   *  Gain: '<S5>/Kb'
   *  Sum: '<S5>/SumI2'
   */
  Attitude_B.SumI1 = (rtb_ProportionalGain_h - rtb_ProportionalGain) *
    Attitude_P.Kbp + Attitude_P.Kip * rtb_Sum4;

  /* Sum: '<S6>/SumI1' incorporates:
   *  Gain: '<S6>/Integral Gain'
   *  Gain: '<S6>/Kb'
   *  Sum: '<S6>/SumI2'
   */
  Attitude_B.SumI1_e = (Ctheta - Cphi) * Attitude_P.Kbq + Attitude_P.Kiq * Sphi;

  /* Sum: '<S7>/SumI1' incorporates:
   *  Gain: '<S7>/Integral Gain'
   *  Gain: '<S7>/Kb'
   *  Sum: '<S7>/SumI2'
   */
  Attitude_B.SumI1_k = (rtb_Saturate_l - rtb_Sum_k) * Attitude_P.Kbr +
    Attitude_P.Kir * rtb_Sum6;
  if (rtmIsMajorTimeStep((&Attitude_M))) {
    rt_ertODEUpdateContinuousStates(&(&Attitude_M)->solverInfo);

    /* Update absolute time for base rate */
    /* The "clockTick0" counts the number of times the code of this task has
     * been executed. The absolute time is the multiplication of "clockTick0"
     * and "Timing.stepSize0". Size of "clockTick0" ensures timer will not
     * overflow during the application lifespan selected.
     * Timer of this task consists of two 32 bit unsigned integers.
     * The two integers represent the low bits Timing.clockTick0 and the high bits
     * Timing.clockTickH0. When the low bit overflows to 0, the high bits increment.
     */
    if (!(++(&Attitude_M)->Timing.clockTick0)) {
      ++(&Attitude_M)->Timing.clockTickH0;
    }

    (&Attitude_M)->Timing.t[0] = rtsiGetSolverStopTime(&(&Attitude_M)
      ->solverInfo);

    {
      /* Update absolute timer for sample time: [0.01s, 0.0s] */
      /* The "clockTick1" counts the number of times the code of this task has
       * been executed. The resolution of this integer timer is 0.01, which is the step size
       * of the task. Size of "clockTick1" ensures timer will not overflow during the
       * application lifespan selected.
       * Timer of this task consists of two 32 bit unsigned integers.
       * The two integers represent the low bits Timing.clockTick1 and the high bits
       * Timing.clockTickH1. When the low bit overflows to 0, the high bits increment.
       */
      (&Attitude_M)->Timing.clockTick1++;
      if (!(&Attitude_M)->Timing.clockTick1) {
        (&Attitude_M)->Timing.clockTickH1++;
      }
    }
  }                                    /* end MajorTimeStep */
}