コード例 #1
0
ファイル: ert_main.c プロジェクト: Derilion/dh_arduino
int main(void)
{
  volatile boolean_T runModel = 1;
  float modelBaseRate = 2.0;
  float systemClock = 0;
  init();
  MW_Arduino_Init();
  rtmSetErrorStatus(test_motor_M, 0);

  /* initialize external mode */
  rtParseArgsForExtMode(0, NULL);
  test_motor_initialize();
  sei();

  /* External mode */
  rtSetTFinalForExtMode(&rtmGetTFinal(test_motor_M));
  rtExtModeCheckInit(1);

  {
    boolean_T rtmStopReq = false;
    rtExtModeWaitForStartPkt(test_motor_M->extModeInfo, 1, &rtmStopReq);
    if (rtmStopReq) {
      rtmSetStopRequested(test_motor_M, true);
    }
  }

  rtERTExtModeStartMsg();
  cli();
  configureArduinoAVRTimer();
  runModel =
    (rtmGetErrorStatus(test_motor_M) == (NULL)) && !rtmGetStopRequested
    (test_motor_M);
  sei();
  sei();
  while (runModel) {
    /* External mode */
    {
      boolean_T rtmStopReq = false;
      rtExtModeOneStep(test_motor_M->extModeInfo, 1, &rtmStopReq);
      if (rtmStopReq) {
        rtmSetStopRequested(test_motor_M, true);
      }
    }

    runModel =
      (rtmGetErrorStatus(test_motor_M) == (NULL)) && !rtmGetStopRequested
      (test_motor_M);
  }

  rtExtModeShutdown(1);

  /* Disable rt_OneStep() here */

  /* Terminate model */
  test_motor_terminate();
  cli();
  return 0;
}
コード例 #2
0
ファイル: ert_main.c プロジェクト: gte999s/QuadCopter
void baseRateTask(void *arg)
{
  baseRateInfo_t info = *((baseRateInfo_t *)arg);
  MW_setTaskPeriod(info.period, info.signo);
  while ((rtmGetErrorStatus(motorControlTask_M) == (NULL)) &&
         !rtmGetStopRequested(motorControlTask_M) ) {
    /* Wait for the next timer interrupt */
    if (MW_sigWaitWithOverrunDetection(&info.sigMask) == 1) {
      printf("Overrun - rate for base rate task too fast.\n");
      fflush(stdout);
    }

    /* External mode */
    {
      boolean_T rtmStopReq = false;
      rtExtModePauseIfNeeded(motorControlTask_M->extModeInfo, 2, &rtmStopReq);
      if (rtmStopReq) {
        rtmSetStopRequested(motorControlTask_M, true);
      }

      if (rtmGetStopRequested(motorControlTask_M) == true) {
        rtmSetErrorStatus(motorControlTask_M, "Simulation finished");
        break;
      }
    }

    /* External mode */
    {
      boolean_T rtmStopReq = false;
      rtExtModeOneStep(motorControlTask_M->extModeInfo, 2, &rtmStopReq);
      if (rtmStopReq) {
        rtmSetStopRequested(motorControlTask_M, true);
      }
    }

    motorControlTask_output();

    /* Get model outputs here */

    /* External mode */
    rtExtModeUploadCheckTrigger(2);

    {                                  /* Sample time: [0.0s, 0.0s] */
      rtExtModeUpload(0, motorControlTask_M->Timing.t[0]);
    }

    {                                  /* Sample time: [0.02s, 0.0s] */
      rtExtModeUpload(1, ((motorControlTask_M->Timing.clockTick1) * 0.02));
    }

    motorControlTask_update();
    rtExtModeCheckEndTrigger();
  }                                    /* while */

  sem_post(&stopSem);
}
コード例 #3
0
void baseRateTask(void *arg)
{
  baseRateInfo_t info = *((baseRateInfo_t *)arg);
  MW_setTaskPeriod(info.period, info.signo);
  while ((rtmGetErrorStatus(Serial_M) == (NULL)) && !rtmGetStopRequested
         (Serial_M) ) {
    /* Wait for the next timer interrupt */
    MW_sigWait(&info.sigMask);

    /* External mode */
    {
      boolean_T rtmStopReq = false;
      rtExtModePauseIfNeeded(Serial_M->extModeInfo, 2, &rtmStopReq);
      if (rtmStopReq) {
        rtmSetStopRequested(Serial_M, true);
      }

      if (rtmGetStopRequested(Serial_M) == true) {
        rtmSetErrorStatus(Serial_M, "Simulation finished");
        break;
      }
    }

    /* External mode */
    {
      boolean_T rtmStopReq = false;
      rtExtModeOneStep(Serial_M->extModeInfo, 2, &rtmStopReq);
      if (rtmStopReq) {
        rtmSetStopRequested(Serial_M, true);
      }
    }

    Serial_output();

    /* Get model outputs here */

    /* External mode */
    rtExtModeUploadCheckTrigger(2);

    {                                  /* Sample time: [0.0s, 0.0s] */
      rtExtModeUpload(0, Serial_M->Timing.t[0]);
    }

    {                                  /* Sample time: [0.05s, 0.0s] */
      rtExtModeUpload(1, ((Serial_M->Timing.clockTick1) * 0.05));
    }

    Serial_update();
    rtExtModeCheckEndTrigger();
  }                                    /* while */

  sem_post(&stopSem);
}
コード例 #4
0
ファイル: ert_main.c プロジェクト: epffpe/Matlab
void baseRateTask(void *arg)
{
  baseRateInfo_t info = *((baseRateInfo_t *)arg);
  MW_setTaskPeriod(info.period, info.signo);
  while ((rtmGetErrorStatus(raspberrypi_audioequalizer_M) == (NULL)) &&
         !rtmGetStopRequested(raspberrypi_audioequalizer_M) ) {
    /* Wait for the next timer interrupt */
    MW_sigWait(&info.sigMask);

    /* External mode */
    {
      boolean_T rtmStopReq = FALSE;
      rtExtModePauseIfNeeded(raspberrypi_audioequalizer_M->extModeInfo, 1,
        &rtmStopReq);
      if (rtmStopReq) {
        rtmSetStopRequested(raspberrypi_audioequalizer_M, TRUE);
      }

      if (rtmGetStopRequested(raspberrypi_audioequalizer_M) == TRUE) {
        rtmSetErrorStatus(raspberrypi_audioequalizer_M, "Simulation finished");
        break;
      }
    }

    /* External mode */
    {
      boolean_T rtmStopReq = FALSE;
      rtExtModeOneStep(raspberrypi_audioequalizer_M->extModeInfo, 1, &rtmStopReq);
      if (rtmStopReq) {
        rtmSetStopRequested(raspberrypi_audioequalizer_M, TRUE);
      }
    }

    raspberrypi_audioequalizer_output();

    /* Get model outputs here */

    /* External mode */
    rtExtModeUploadCheckTrigger(1);

    {                                  /* Sample time: [0.1s, 0.0s] */
      rtExtModeUpload(0, raspberrypi_audioequalizer_M->Timing.taskTime0);
    }

    raspberrypi_audioequalizer_update();
    rtExtModeCheckEndTrigger();
  }                                    /* while */

  sem_post(&stopSem);
}
コード例 #5
0
ファイル: CelpSimulink.c プロジェクト: georgiee/lip-sync-lpc
/* Model update function */
static void CelpSimulink_update(int_T tid)
{
  {
    static char sErr[512];
    void *device = CelpSimulink_DWork.ToAudioDevice_AudioDevice;
    AudioDeviceLibrary *adl = (AudioDeviceLibrary*)
      &CelpSimulink_DWork.ToAudioDevice_AudioDeviceLib[0];
    sErr[0] = 0;
    if (device)
      adl->deviceUpdate(device, sErr, CelpSimulink_B.DeemphasisFilter, 1, 1);
    if (*sErr) {
      DestroyAudioDeviceLibrary(adl);
      rtmSetErrorStatus(CelpSimulink_M, sErr);
      rtmSetStopRequested(CelpSimulink_M, 1);
    }
  }

  /* Update absolute time for base rate */
  if (!(++CelpSimulink_M->Timing.clockTick0))
    ++CelpSimulink_M->Timing.clockTickH0;
  CelpSimulink_M->Timing.t[0] = CelpSimulink_M->Timing.clockTick0 *
    CelpSimulink_M->Timing.stepSize0 + CelpSimulink_M->Timing.clockTickH0 *
    CelpSimulink_M->Timing.stepSize0 * 4294967296.0;
  UNUSED_PARAMETER(tid);
}
コード例 #6
0
void baseRateTask(void *arg)
{
  runModel = (rtmGetErrorStatus(Model_M) == (NULL)) && !rtmGetStopRequested
    (Model_M);
  while (runModel) {
    mw_osSemaphoreWaitEver(&baserateTaskSem);

    /* External mode */
    {
      boolean_T rtmStopReq = false;
      rtExtModePauseIfNeeded(Model_M->extModeInfo, 1, &rtmStopReq);
      if (rtmStopReq) {
        rtmSetStopRequested(Model_M, true);
      }

      if (rtmGetStopRequested(Model_M) == true) {
        rtmSetErrorStatus(Model_M, "Simulation finished");
        break;
      }
    }

    Model_step();

    /* Get model outputs here */
    rtExtModeCheckEndTrigger();
    runModel = (rtmGetErrorStatus(Model_M) == (NULL)) && !rtmGetStopRequested
      (Model_M);
  }

  runModel = 0;
  terminateTask(arg);
  taskDelete((void *)0);
}
コード例 #7
0
int main(int argc, char **argv)
{
  printf("**starting the model**\n");
  fflush(stdout);
  rtmSetErrorStatus(Model_M, 0);
  rtExtModeParseArgs(argc, (const char_T **)argv, NULL);

  /* Initialize model */
  Model_initialize();

  /* External mode */
  rtSetTFinalForExtMode(&rtmGetTFinal(Model_M));
  rtExtModeCheckInit(1);

  {
    boolean_T rtmStopReq = false;
    rtExtModeWaitForStartPkt(Model_M->extModeInfo, 1, &rtmStopReq);
    if (rtmStopReq) {
      rtmSetStopRequested(Model_M, true);
    }
  }

  rtERTExtModeStartMsg();

  /* Call RTOS Initialization funcation */
  prosRTOSInit(0.2, 0);

  /* Wait for stop semaphore */
  mw_osSemaphoreWaitEver(&stopSem);
  return 0;
}
コード例 #8
0
void backgroundTask(int sig)
{
  while (runModel) {
    /* External mode */
    {
      boolean_T rtmStopReq = false;
      rtExtModeOneStep(Model_M->extModeInfo, 1, &rtmStopReq);
      if (rtmStopReq) {
        rtmSetStopRequested(Model_M, true);
      }
    }
  }
}
コード例 #9
0
ファイル: udpRead.c プロジェクト: gte999s/QuadCopter
/* Model terminate function */
void udpRead_terminate(void)
{
  char_T *sErr;

  /* Terminate for S-Function (sdspFromNetwork): '<Root>/UDP Receive' */
  sErr = GetErrorBuffer(&udpRead_DW.UDPReceive_NetworkLib[0U]);
  LibTerminate(&udpRead_DW.UDPReceive_NetworkLib[0U]);
  if (*sErr != 0) {
    rtmSetErrorStatus(udpRead_M, sErr);
    rtmSetStopRequested(udpRead_M, 1);
  }

  LibDestroy(&udpRead_DW.UDPReceive_NetworkLib[0U], 0);
  DestroyUDPInterface(&udpRead_DW.UDPReceive_NetworkLib[0U]);

  /* End of Terminate for S-Function (sdspFromNetwork): '<Root>/UDP Receive' */
}
コード例 #10
0
/* Model terminate function */
void beagleboard_communication_terminate(void)
{
  char_T *sErr;

  /* Terminate for S-Function (sdspToNetwork): '<Root>/UDP Send' */
  sErr = GetErrorBuffer(&beagleboard_communication_DWork.UDPSend_NetworkLib[0U]);
  LibTerminate(&beagleboard_communication_DWork.UDPSend_NetworkLib[0U]);
  if (*sErr != 0) {
    rtmSetErrorStatus(beagleboard_communication_M, sErr);
    rtmSetStopRequested(beagleboard_communication_M, 1);
  }

  LibDestroy(&beagleboard_communication_DWork.UDPSend_NetworkLib[0U], 1);
  DestroyUDPInterface(&beagleboard_communication_DWork.UDPSend_NetworkLib[0U]);

  /* End of Terminate for S-Function (sdspToNetwork): '<Root>/UDP Send' */
}
コード例 #11
0
/* Model initialize function */
void beagleboard_communication_initialize(void)
{
  /* Registration code */

  /* initialize real-time model */
  (void) memset((void *)beagleboard_communication_M, 0,
                sizeof(RT_MODEL_beagleboard_communicat));

  /* block I/O */
  (void) memset(((void *) &beagleboard_communication_B), 0,
                sizeof(BlockIO_beagleboard_communicati));

  /* states (dwork) */
  (void) memset((void *)&beagleboard_communication_DWork, 0,
                sizeof(D_Work_beagleboard_communicatio));

  {
    char_T *sErr;

    /* Start for S-Function (sdspToNetwork): '<Root>/UDP Send' */
    sErr = GetErrorBuffer(&beagleboard_communication_DWork.UDPSend_NetworkLib[0U]);
    CreateUDPInterface(&beagleboard_communication_DWork.UDPSend_NetworkLib[0U]);
    if (*sErr == 0) {
      LibCreate_Network(&beagleboard_communication_DWork.UDPSend_NetworkLib[0U],
                        1, "255.255.255.255", -1, "192.168.1.35", 25000, 8192, 8,
                        0);
    }

    if (*sErr == 0) {
      LibStart(&beagleboard_communication_DWork.UDPSend_NetworkLib[0U]);
    }

    if (*sErr != 0) {
      DestroyUDPInterface(&beagleboard_communication_DWork.UDPSend_NetworkLib[0U]);
      if (*sErr != 0) {
        rtmSetErrorStatus(beagleboard_communication_M, sErr);
        rtmSetStopRequested(beagleboard_communication_M, 1);
      }
    }

    /* End of Start for S-Function (sdspToNetwork): '<Root>/UDP Send' */
  }
}
コード例 #12
0
ファイル: CelpSimulink.c プロジェクト: georgiee/lip-sync-lpc
void MdlStart(void)
{
  /* Start for iterator SubSystem: '<Root>/Codebook Search' */

  /* InitializeConditions for S-Function (sdspstatminmax): '<S6>/Maximum1' */
  CelpSimulink_DWork.Maximum1_Valdata = rtMinusInf;

  /* InitializeConditions for UnitDelay: '<S5>/Unit Delay' */
  CelpSimulink_DWork.UnitDelay_DSTATE = CelpSimulink_P.UnitDelay_X0;

  /* end of Start for SubSystem: '<Root>/Codebook Search' */
  {
    /* Signal Processing Blockset ToAudioDevice (sdspToAudioDevice) - '<Root>/To Audio Device' - Start */
    void *device = NULL;
    AudioDeviceLibrary adl;
    static char sErr[512];
    sErr[0] = 0;
    adl = CreateAudioDeviceLibrary(&sErr[0]);
    memcpy(&CelpSimulink_DWork.ToAudioDevice_AudioDeviceLib[0], &adl, sizeof
           (AudioDeviceLibrary));
    if (!*sErr) {                      /* Create the device */
      adl.deviceCreate(&sErr[0], NULL, "Default", 1, &device,
                       80, 8000.0, 1, 512, 8000.0,
                       1, 1);
    }

    if (!*sErr) {
      adl.deviceStart(device, &sErr[0]);
    }

    if (*sErr) {
      DestroyAudioDeviceLibrary(&adl);
      rtmSetErrorStatus(CelpSimulink_M, sErr);
      rtmSetStopRequested(CelpSimulink_M, 1);
    }

    CelpSimulink_DWork.ToAudioDevice_AudioDevice = device;
  }

  MdlInitialize();
}
コード例 #13
0
/* Model update function */
void beagleboard_communication_update(void)
{
  char_T *sErr;

  /* Update for Sin: '<Root>/Sine Wave' */
  beagleboard_communication_DWork.counter++;
  if (beagleboard_communication_DWork.counter ==
      beagleboard_communication_P.SineWave_NumSamp) {
    beagleboard_communication_DWork.counter = 0;
  }

  /* End of Update for Sin: '<Root>/Sine Wave' */

  /* Update for S-Function (sdspToNetwork): '<Root>/UDP Send' */
  sErr = GetErrorBuffer(&beagleboard_communication_DWork.UDPSend_NetworkLib[0U]);
  LibUpdate_Network(&beagleboard_communication_DWork.UDPSend_NetworkLib[0U],
                    &beagleboard_communication_B.sine_wave, 1);
  if (*sErr != 0) {
    rtmSetErrorStatus(beagleboard_communication_M, sErr);
    rtmSetStopRequested(beagleboard_communication_M, 1);
  }

  /* End of Update for S-Function (sdspToNetwork): '<Root>/UDP Send' */
}
コード例 #14
0
/* Model output function */
void omni_interface_output(int_T tid)
{
  /* local block i/o variables */
  real_T rtb_HILRead_o3[2];
  real_T rtb_Saturation[3];
  real_T rtb_StreamRead1_o2[3];
  t_stream_ptr rtb_StreamAnswer_o1;
  t_stream_ptr rtb_StreamWrite_o1;
  t_stream_ptr rtb_StreamAnswer1_o1;
  t_stream_ptr rtb_StreamRead1_o1;
  uint8_T rtb_StreamAnswer1_o2;
  boolean_T rtb_LogicalOperator2[2];
  boolean_T rtb_StreamRead1_o4;
  boolean_T rtb_StreamRead1_o3;

  /* S-Function Block: omni_interface/HIL Read (hil_read_block) */
  {
    t_error result = hil_read(omni_interface_DWork.HILInitialize_Card,
      omni_interface_P.HILRead_AnalogChannels, 3U,
      omni_interface_P.HILRead_EncoderChannels, 3U,
      omni_interface_P.HILRead_DigitalChannels, 2U,
      NULL, 0U,
      rtb_Saturation,
      &omni_interface_DWork.HILRead_EncoderBuffer[0],
      &omni_interface_DWork.HILRead_DigitalBuffer[0],
      NULL
      );
    if (result < 0) {
      msg_get_error_messageA(NULL, result, _rt_error_message, sizeof
        (_rt_error_message));
      rtmSetErrorStatus(omni_interface_M, _rt_error_message);
    } else {
      rtb_StreamRead1_o2[0] = omni_interface_DWork.HILRead_EncoderBuffer[0];
      rtb_StreamRead1_o2[1] = omni_interface_DWork.HILRead_EncoderBuffer[1];
      rtb_StreamRead1_o2[2] = omni_interface_DWork.HILRead_EncoderBuffer[2];
      rtb_HILRead_o3[0] = omni_interface_DWork.HILRead_DigitalBuffer[0];
      rtb_HILRead_o3[1] = omni_interface_DWork.HILRead_DigitalBuffer[1];
    }
  }

  /* Gain: '<Root>/Gain2' incorporates:
   *  Bias: '<Root>/Bias3'
   *  Gain: '<Root>/Gain1'
   *  Gain: '<Root>/Gear Ratio'
   */
  omni_interface_B.Gain2[0] = (rtb_StreamRead1_o2[0] +
    omni_interface_P.Bias3_Bias[0]) * omni_interface_P.Gain1_Gain *
    omni_interface_P.GearRatio_Gain[0] * omni_interface_P.Gain2_Gain;
  omni_interface_B.Gain2[1] = (rtb_StreamRead1_o2[1] +
    omni_interface_P.Bias3_Bias[1]) * omni_interface_P.Gain1_Gain *
    omni_interface_P.GearRatio_Gain[1] * omni_interface_P.Gain2_Gain;
  omni_interface_B.Gain2[2] = (rtb_StreamRead1_o2[2] +
    omni_interface_P.Bias3_Bias[2]) * omni_interface_P.Gain1_Gain *
    omni_interface_P.GearRatio_Gain[2] * omni_interface_P.Gain2_Gain;

  /* Switch: '<S8>/Reset' incorporates:
   *  Constant: '<S8>/Initial Condition'
   *  MinMax: '<S2>/MinMax'
   *  UnitDelay: '<S8>/FixPt Unit Delay1'
   */
  if (0.0 != 0.0) {
    omni_interface_B.Reset = omni_interface_P.InitialCondition_Value;
  } else {
    omni_interface_B.Reset = rt_MIN(omni_interface_B.Gain2[0],
      omni_interface_DWork.FixPtUnitDelay1_DSTATE);
  }

  /* Switch: '<S9>/Reset' incorporates:
   *  Constant: '<S9>/Initial Condition'
   *  MinMax: '<S3>/MinMax'
   *  UnitDelay: '<S9>/FixPt Unit Delay1'
   */
  if (0.0 != 0.0) {
    omni_interface_B.Reset_c = omni_interface_P.InitialCondition_Value_d;
  } else {
    omni_interface_B.Reset_c = rt_MIN(omni_interface_B.Gain2[1],
      omni_interface_DWork.FixPtUnitDelay1_DSTATE_i);
  }

  /* Switch: '<S10>/Reset' incorporates:
   *  Constant: '<S10>/Initial Condition'
   *  MinMax: '<S4>/MinMax'
   *  UnitDelay: '<S10>/FixPt Unit Delay1'
   */
  if (0.0 != 0.0) {
    omni_interface_B.Reset_n = omni_interface_P.InitialCondition_Value_g;
  } else {
    omni_interface_B.Reset_n = rt_MIN(omni_interface_B.Gain2[2],
      omni_interface_DWork.FixPtUnitDelay1_DSTATE_a);
  }

  /* Switch: '<S11>/Reset' incorporates:
   *  Constant: '<S11>/Initial Condition'
   *  MinMax: '<S5>/MinMax'
   *  UnitDelay: '<S11>/FixPt Unit Delay1'
   */
  if (0.0 != 0.0) {
    omni_interface_B.Reset_c3 = omni_interface_P.InitialCondition_Value_gd;
  } else {
    omni_interface_B.Reset_c3 = rt_MAX(omni_interface_B.Gain2[0],
      omni_interface_DWork.FixPtUnitDelay1_DSTATE_e);
  }

  /* Switch: '<S12>/Reset' incorporates:
   *  Constant: '<S12>/Initial Condition'
   *  MinMax: '<S6>/MinMax'
   *  UnitDelay: '<S12>/FixPt Unit Delay1'
   */
  if (0.0 != 0.0) {
    omni_interface_B.Reset_l = omni_interface_P.InitialCondition_Value_m;
  } else {
    omni_interface_B.Reset_l = rt_MAX(omni_interface_B.Gain2[1],
      omni_interface_DWork.FixPtUnitDelay1_DSTATE_eh);
  }

  /* Switch: '<S13>/Reset' incorporates:
   *  Constant: '<S13>/Initial Condition'
   *  MinMax: '<S7>/MinMax'
   *  UnitDelay: '<S13>/FixPt Unit Delay1'
   */
  if (0.0 != 0.0) {
    omni_interface_B.Reset_i = omni_interface_P.InitialCondition_Value_f;
  } else {
    omni_interface_B.Reset_i = rt_MAX(omni_interface_B.Gain2[2],
      omni_interface_DWork.FixPtUnitDelay1_DSTATE_d);
  }

  /* S-Function Block: omni_interface/Stream Answer (stream_answer_block) */
  {
    static const t_short endian_test = 0x0201;
    t_error result = 0;
    t_boolean close_listener = (FALSE != 0);
    t_boolean close_client = (FALSE != 0);
    rtb_StreamAnswer_o1 = NULL;
    switch (omni_interface_DWork.StreamAnswer_State) {
     case STREAM_ANSWER_STATE_NOT_LISTENING:
      {
        if (!close_listener) {
          result = stream_listen("tcpip://localhost:18000", true,
            &omni_interface_DWork.StreamAnswer_Listener);
          if (result == 0) {
            omni_interface_DWork.StreamAnswer_State =
              STREAM_ANSWER_STATE_NOT_CONNECTED;
          }
        }
        break;
      }

     case STREAM_ANSWER_STATE_NOT_CONNECTED:
      {
        if (!close_client) {
          result = stream_accept(omni_interface_DWork.StreamAnswer_Listener,
            omni_interface_P.StreamAnswer_SendBufferSize,
            omni_interface_P.StreamAnswer_ReceiveBufferSize,
            &omni_interface_DWork.StreamAnswer_Client);
          if (result == 0) {
            omni_interface_DWork.StreamAnswer_State =
              STREAM_ANSWER_STATE_CONNECTED;
            stream_set_swap_bytes(omni_interface_DWork.StreamAnswer_Client,
                                  *(t_byte *) &endian_test !=
                                  omni_interface_P.StreamAnswer_Endian);
            rtb_StreamAnswer_o1 = &omni_interface_DWork.StreamAnswer_Client;
          }
        }
        break;
      }

     case STREAM_ANSWER_STATE_CONNECTED:
      {
        rtb_StreamAnswer_o1 = &omni_interface_DWork.StreamAnswer_Client;
        if (!close_client) {
          break;
        }

        /* Fall through deliberately */
      }

     default:
      {
        t_error close_result = stream_close
          (omni_interface_DWork.StreamAnswer_Client);
        if (close_result == 0) {
          omni_interface_DWork.StreamAnswer_Client = NULL;
          omni_interface_DWork.StreamAnswer_State =
            STREAM_ANSWER_STATE_NOT_CONNECTED;
          rtb_StreamAnswer_o1 = NULL;
          if (close_listener) {
            close_result = stream_close
              (omni_interface_DWork.StreamAnswer_Listener);
            if (close_result == 0) {
              omni_interface_DWork.StreamAnswer_Listener = NULL;
              omni_interface_DWork.StreamAnswer_State =
                STREAM_ANSWER_STATE_NOT_LISTENING;
            } else if (result == 0) {
              result = close_result;
            }
          }
        } else if (result == 0) {
          result = close_result;
        }
        break;
      }
    }

    rtb_StreamAnswer1_o2 = omni_interface_DWork.StreamAnswer_State;
    omni_interface_B.StreamAnswer_o3 = (int32_T) result;
  }

  /* Logic: '<Root>/Logical Operator' incorporates:
   *  Logic: '<Root>/Logical Operator1'
   *  RelationalOperator: '<S1>/Compare'
   */
  omni_interface_B.LogicalOperator = ((rtb_StreamAnswer1_o2 != 0U) &&
    (!((omni_interface_B.StreamAnswer_o3 != 0) != 0U)));

  /* S-Function (stream_write_block): '<Root>/Stream Write' */
  {
    t_error result;
    if (rtb_StreamAnswer_o1 != NULL) {
      result = stream_send_unit_array(*rtb_StreamAnswer_o1,
        omni_interface_B.Gain2, sizeof(real_T), 3);
      if (result == 1) {
        stream_flush(*rtb_StreamAnswer_o1);
      }

      if (result == -QERR_WOULD_BLOCK) {
        result = 0;
      }
    }

    rtb_StreamWrite_o1 = rtb_StreamAnswer_o1;
  }

  /* S-Function (stream_read_block): '<Root>/Stream Read' */
  /* S-Function Block: omni_interface/Stream Read (stream_read_block) */
  {
    t_error result;
    if (rtb_StreamWrite_o1 != NULL) {
      result = stream_receive_unit_array(*rtb_StreamWrite_o1,
        omni_interface_B.StreamRead_o2, sizeof(real_T), 3);
      omni_interface_B.StreamRead_o3 = (result > 0);
      if (result == -QERR_WOULD_BLOCK) {
        result = 0;
      }

      /*
         if (result <= 0 && result != -QERR_WOULD_BLOCK) {
         memset(omni_interface_B.StreamRead_o2, 0, 3 * sizeof(real_T));
         }
       */
    } else {
      memset(omni_interface_B.StreamRead_o2, 0, 3 * sizeof(real_T));
      omni_interface_B.StreamRead_o3 = false;
      result = 0;
    }

    rtb_StreamRead1_o1 = rtb_StreamWrite_o1;
  }

  /* S-Function Block: omni_interface/Stream Answer1 (stream_answer_block) */
  {
    static const t_short endian_test = 0x0201;
    t_error result = 0;
    t_boolean close_listener = (FALSE != 0);
    t_boolean close_client = (FALSE != 0);
    rtb_StreamAnswer1_o1 = NULL;
    switch (omni_interface_DWork.StreamAnswer1_State) {
     case STREAM_ANSWER_STATE_NOT_LISTENING:
      {
        if (!close_listener) {
          result = stream_listen("tcpip://localhost:18002", true,
            &omni_interface_DWork.StreamAnswer1_Listener);
          if (result == 0) {
            omni_interface_DWork.StreamAnswer1_State =
              STREAM_ANSWER_STATE_NOT_CONNECTED;
          }
        }
        break;
      }

     case STREAM_ANSWER_STATE_NOT_CONNECTED:
      {
        if (!close_client) {
          result = stream_accept(omni_interface_DWork.StreamAnswer1_Listener,
            omni_interface_P.StreamAnswer1_SendBufferSize,
            omni_interface_P.StreamAnswer1_ReceiveBufferSize,
            &omni_interface_DWork.StreamAnswer1_Client);
          if (result == 0) {
            omni_interface_DWork.StreamAnswer1_State =
              STREAM_ANSWER_STATE_CONNECTED;
            stream_set_swap_bytes(omni_interface_DWork.StreamAnswer1_Client,
                                  *(t_byte *) &endian_test !=
                                  omni_interface_P.StreamAnswer1_Endian);
            rtb_StreamAnswer1_o1 = &omni_interface_DWork.StreamAnswer1_Client;
          }
        }
        break;
      }

     case STREAM_ANSWER_STATE_CONNECTED:
      {
        rtb_StreamAnswer1_o1 = &omni_interface_DWork.StreamAnswer1_Client;
        if (!close_client) {
          break;
        }

        /* Fall through deliberately */
      }

     default:
      {
        t_error close_result = stream_close
          (omni_interface_DWork.StreamAnswer1_Client);
        if (close_result == 0) {
          omni_interface_DWork.StreamAnswer1_Client = NULL;
          omni_interface_DWork.StreamAnswer1_State =
            STREAM_ANSWER_STATE_NOT_CONNECTED;
          rtb_StreamAnswer1_o1 = NULL;
          if (close_listener) {
            close_result = stream_close
              (omni_interface_DWork.StreamAnswer1_Listener);
            if (close_result == 0) {
              omni_interface_DWork.StreamAnswer1_Listener = NULL;
              omni_interface_DWork.StreamAnswer1_State =
                STREAM_ANSWER_STATE_NOT_LISTENING;
            } else if (result == 0) {
              result = close_result;
            }
          }
        } else if (result == 0) {
          result = close_result;
        }
        break;
      }
    }

    omni_interface_B.StreamAnswer1_o3 = (int32_T) result;
  }

  /* Gain: '<Root>/Gain' */
  rtb_StreamRead1_o2[0] = omni_interface_P.Gain_Gain[0] *
    omni_interface_B.StreamRead_o2[0];
  rtb_StreamRead1_o2[1] = omni_interface_P.Gain_Gain[1] *
    omni_interface_B.StreamRead_o2[1];
  rtb_StreamRead1_o2[2] = omni_interface_P.Gain_Gain[2] *
    omni_interface_B.StreamRead_o2[2];

  /* Product: '<Root>/Product' */
  rtb_Saturation[0] = (int32_T)omni_interface_B.LogicalOperator ?
    rtb_StreamRead1_o2[0] : 0.0;
  rtb_Saturation[1] = (int32_T)omni_interface_B.LogicalOperator ?
    rtb_StreamRead1_o2[1] : 0.0;
  rtb_Saturation[2] = (int32_T)omni_interface_B.LogicalOperator ?
    rtb_StreamRead1_o2[2] : 0.0;

  /* Saturate: '<Root>/Saturation' */
  rtb_Saturation[0] = rt_SATURATE(rtb_Saturation[0],
    omni_interface_P.Saturation_LowerSat, omni_interface_P.Saturation_UpperSat);
  rtb_Saturation[1] = rt_SATURATE(rtb_Saturation[1],
    omni_interface_P.Saturation_LowerSat, omni_interface_P.Saturation_UpperSat);
  rtb_Saturation[2] = rt_SATURATE(rtb_Saturation[2],
    omni_interface_P.Saturation_LowerSat, omni_interface_P.Saturation_UpperSat);

  /* S-Function Block: omni_interface/HIL Write (hil_write_block) */
  {
    t_error result;
    result = hil_write(omni_interface_DWork.HILInitialize_Card,
                       NULL, 0U,
                       omni_interface_P.HILWrite_PWMChannels, 3U,
                       NULL, 0U,
                       NULL, 0U,
                       NULL,
                       rtb_Saturation,
                       NULL,
                       NULL
                       );
    if (result < 0) {
      msg_get_error_messageA(NULL, result, _rt_error_message, sizeof
        (_rt_error_message));
      rtmSetErrorStatus(omni_interface_M, _rt_error_message);
    }
  }

  /* S-Function (stream_read_block): '<Root>/Stream Read1' */
  /* S-Function Block: omni_interface/Stream Read1 (stream_read_block) */
  {
    t_error result;
    if (rtb_StreamAnswer1_o1 != NULL) {
      result = stream_receive_unit_array(*rtb_StreamAnswer1_o1,
        rtb_StreamRead1_o2, sizeof(real_T), 3);
      if (result == -QERR_WOULD_BLOCK) {
        result = 0;
      }

      /*
         if (result <= 0 && result != -QERR_WOULD_BLOCK) {
         memset(rtb_StreamRead1_o2, 0, 3 * sizeof(real_T));
         }
       */
    } else {
      memset(rtb_StreamRead1_o2, 0, 3 * sizeof(real_T));
      result = 0;
    }

    rtb_StreamRead1_o1 = rtb_StreamAnswer1_o1;
  }

  /* Logic: '<Root>/Logical Operator2' */
  rtb_LogicalOperator2[0] = ((rtb_HILRead_o3[0] != 0.0) || (0.0 != 0.0) ||
    (rtb_StreamRead1_o2[1] != 0.0));
  rtb_LogicalOperator2[1] = ((rtb_HILRead_o3[1] != 0.0) || (0.0 != 0.0) ||
    (rtb_StreamRead1_o2[1] != 0.0));

  /* Stop: '<Root>/Stop Simulation' */
  if (rtb_LogicalOperator2[0] || rtb_LogicalOperator2[1]) {
    rtmSetStopRequested(omni_interface_M, 1);
  }

  UNUSED_PARAMETER(tid);
}
コード例 #15
0
ファイル: ert_main.c プロジェクト: epffpe/Matlab
int main(int argc, char **argv)
{
  int ret;
  baseRateInfo_t info;
  pthread_attr_t attr;
  pthread_t baseRateThread;
  size_t stackSize;
  unsigned long cpuMask = 0x1;
  unsigned int len = sizeof(cpuMask);
  printf("**starting the model**\n");
  fflush(stdout);
  rtmSetErrorStatus(raspberrypi_audioequalizer_M, 0);
  rtExtModeParseArgs(argc, (const char_T **)argv, NULL);

  /* All threads created by this process must run on a single CPU */
  ret = sched_setaffinity(0, len, (cpu_set_t *) &cpuMask);
  CHECK_STATUS(ret, "sched_setaffinity");

  /* Initialize semaphore used for thread synchronization */
  ret = sem_init(&stopSem, 0, 0);
  CHECK_STATUS(ret, "sem_init:stopSem");

  /* Create threads executing the Simulink model */
  pthread_attr_init(&attr);
  ret = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
  CHECK_STATUS(ret, "pthread_attr_setinheritsched");
  ret = pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);
  CHECK_STATUS(ret, "pthread_attr_setdetachstate");

  /* PTHREAD_STACK_MIN is the minimum stack size required to start a thread */
  stackSize = 64000 + PTHREAD_STACK_MIN;
  ret = pthread_attr_setstacksize(&attr, stackSize);
  CHECK_STATUS(ret, "pthread_attr_setstacksize");

  /* Block signal used for timer notification */
  info.period = 0.1;
  info.signo = SIGRTMIN;
  sigemptyset(&info.sigMask);
  MW_blockSignal(info.signo, &info.sigMask);
  signal(SIGTERM, MW_exitHandler);     /* kill */
  signal(SIGHUP, MW_exitHandler);      /* kill -HUP */
  signal(SIGINT, MW_exitHandler);      /* Interrupt from keyboard */
  signal(SIGQUIT, MW_exitHandler);     /* Quit from keyboard */
  raspberrypi_audioequalizer_initialize();

  /* External mode */
  rtSetTFinalForExtMode(&rtmGetTFinal(raspberrypi_audioequalizer_M));
  rtExtModeCheckInit(1);

  {
    boolean_T rtmStopReq = FALSE;
    rtExtModeWaitForStartPkt(raspberrypi_audioequalizer_M->extModeInfo, 1,
      &rtmStopReq);
    if (rtmStopReq) {
      rtmSetStopRequested(raspberrypi_audioequalizer_M, TRUE);
    }
  }

  rtERTExtModeStartMsg();

  /* Create base rate task */
  ret = pthread_create(&baseRateThread, &attr, (void *) baseRateTask, (void *)
                       &info);
  CHECK_STATUS(ret, "pthread_create");
  pthread_attr_destroy(&attr);

  /* Wait for a stopping condition. */
  MW_sem_wait(&stopSem);

  /* Received stop signal */
  printf("**stopping the model**\n");
  if (rtmGetErrorStatus(raspberrypi_audioequalizer_M) != NULL) {
    printf("\n**%s**\n", rtmGetErrorStatus(raspberrypi_audioequalizer_M));
  }

  /* External mode shutdown */
  rtExtModeShutdown(1);

  /* Disable rt_OneStep() here */

  /* Terminate model */
  raspberrypi_audioequalizer_terminate();
  return 0;
}
コード例 #16
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);
}
コード例 #17
0
ファイル: CelpSimulink.c プロジェクト: georgiee/lip-sync-lpc
void MdlInitialize(void)
{
  {
    /* Signal Processing Blockset From Wave File (sdspwafi2) - '<Root>/From Wave File' - Initialize */
    MWDSP_Wafi_FunctionPtrStruct* fcnPtrs = (MWDSP_Wafi_FunctionPtrStruct*)
      &CelpSimulink_DWork.FromWaveFile_RuntimeFcnPtrs[0];

    /* destroy previous runtime object, if there is one */
    if (fcnPtrs != NULL && fcnPtrs->terminateFcn != NULL)
      fcnPtrs->terminateFcn(CelpSimulink_DWork.FromWaveFile_FromWaveFileObj);
    CelpSimulink_DWork.FromWaveFile_FromWaveFileObj = exMWDSP_Wafi_Create(
      "C:\\Documents and Settings\\Maquina\\Meus documentos\\Arthur\\UFRGS\\DSP\\CELP\\audio.wav",
      16U, 80, 1, 80, 8000.0, 1);
    if (CelpSimulink_DWork.FromWaveFile_FromWaveFileObj == NULL) {
      const char* errMsg = exMWDSP_Wafi_GetErrorMessage();
      rtmSetErrorStatus(CelpSimulink_M, errMsg);
      rtmSetStopRequested(CelpSimulink_M, 1);
    } else {
      exMWDSP_Wafi_GetFunctionPtrs(fcnPtrs);
    }
  }

  /* Signal Processing Blockset Filter Implementation (sdspfilter2) - '<S2>/Pre-Emphasis' */
  /* FIR, Direct-form */
  {
    real32_T *statePtr = (real32_T *)
      &CelpSimulink_DWork.PreEmphasis_FILT_STATES[0];

    /* Scalar expansion of ICs with extra zero element per channel */
    *statePtr++ = *(const real32_T *)&CelpSimulink_ConstP.pooled1;
    *statePtr++ = 0.0F;
  }

  /* Copy ICs into circular buffer */
  {
    const int_T bufLenBytes = 80 * sizeof(real32_T);
    byte_T *circBufPtr = (byte_T *)
      &CelpSimulink_DWork.OverlapAnalysisWindows_CircBuff[0];
    const byte_T *icPtr = (const byte_T *)&CelpSimulink_ConstP.pooled1;
    int_T i = 1;
    while (i-- > 0) {
      MWDSP_CopyScalarICs(circBufPtr, icPtr, 80, sizeof(real32_T));
      circBufPtr += bufLenBytes;
    }
  }

  /* Signal Processing Blockset Filter Implementation (sdspfilter2) - '<S3>/Time-Varying Synthesis Filter' */
  /* All-pole, Direct-form (a0 ~= 1) */
  {
    real32_T *statePtr = (real32_T *)
      &CelpSimulink_DWork.TimeVaryingSynthesisFilter_FILT[0];

    /* Scalar expansion of ICs with extra zero element per channel */
    int_T chanCount = 80;
    while (chanCount--) {
      int_T numElems= 10;
      while (numElems--) {
        *statePtr++ = *(const real32_T *)&CelpSimulink_ConstP.pooled1;
      }

      *statePtr++ = 0.0F;
    }
  }

  /* Signal Processing Blockset Filter Implementation (sdspfilter2) - '<S3>/De-emphasis Filter' */
  /* All-pole, Direct-form (a0 == 1) */
  {
    real32_T *statePtr = (real32_T *)
      &CelpSimulink_DWork.DeemphasisFilter_FILT_STATES[0];

    /* Scalar expansion of ICs with extra zero element per channel */
    int_T chanCount = 80;
    while (chanCount--) {
      *statePtr++ = *(const real32_T *)&CelpSimulink_ConstP.pooled1;
      *statePtr++ = 0.0F;
    }
  }
}
コード例 #18
0
ファイル: udpRead.c プロジェクト: gte999s/QuadCopter
/* Model initialize function */
void udpRead_initialize(void)
{
  /* Registration code */

  /* initialize non-finites */
  rt_InitInfAndNaN(sizeof(real_T));

  /* initialize real-time model */
  (void) memset((void *)udpRead_M, 0,
                sizeof(RT_MODEL_udpRead_T));
  rtmSetTFinal(udpRead_M, -1);
  udpRead_M->Timing.stepSize0 = 0.001;

  /* External mode info */
  udpRead_M->Sizes.checksums[0] = (597430241U);
  udpRead_M->Sizes.checksums[1] = (1019470990U);
  udpRead_M->Sizes.checksums[2] = (4143940322U);
  udpRead_M->Sizes.checksums[3] = (93090447U);

  {
    static const sysRanDType rtAlwaysEnabled = SUBSYS_RAN_BC_ENABLE;
    static RTWExtModeInfo rt_ExtModeInfo;
    static const sysRanDType *systemRan[5];
    udpRead_M->extModeInfo = (&rt_ExtModeInfo);
    rteiSetSubSystemActiveVectorAddresses(&rt_ExtModeInfo, systemRan);
    systemRan[0] = &rtAlwaysEnabled;
    systemRan[1] = (sysRanDType *)&udpRead_DW.ForIteratorSubsystem_SubsysRanB;
    systemRan[2] = (sysRanDType *)&udpRead_DW.ForIteratorSubsystem1_SubsysRan;
    systemRan[3] = (sysRanDType *)&udpRead_DW.EnabledSubsystem_SubsysRanBC;
    systemRan[4] = &rtAlwaysEnabled;
    rteiSetModelMappingInfoPtr(udpRead_M->extModeInfo,
      &udpRead_M->SpecialInfo.mappingInfo);
    rteiSetChecksumsPtr(udpRead_M->extModeInfo, udpRead_M->Sizes.checksums);
    rteiSetTPtr(udpRead_M->extModeInfo, rtmGetTPtr(udpRead_M));
  }

  /* block I/O */
  (void) memset(((void *) &udpRead_B), 0,
                sizeof(B_udpRead_T));

  /* states (dwork) */
  (void) memset((void *)&udpRead_DW, 0,
                sizeof(DW_udpRead_T));

  /* data type transition information */
  {
    static DataTypeTransInfo dtInfo;
    (void) memset((char_T *) &dtInfo, 0,
                  sizeof(dtInfo));
    udpRead_M->SpecialInfo.mappingInfo = (&dtInfo);
    dtInfo.numDataTypes = 14;
    dtInfo.dataTypeSizes = &rtDataTypeSizes[0];
    dtInfo.dataTypeNames = &rtDataTypeNames[0];

    /* Block I/O transition table */
    dtInfo.B = &rtBTransTable;

    /* Parameters transition table */
    dtInfo.P = &rtPTransTable;
  }

  {
    char_T *sErr;

    /* Start for S-Function (sdspFromNetwork): '<Root>/UDP Receive' */
    sErr = GetErrorBuffer(&udpRead_DW.UDPReceive_NetworkLib[0U]);
    CreateUDPInterface(&udpRead_DW.UDPReceive_NetworkLib[0U]);
    if (*sErr == 0) {
      LibCreate_Network(&udpRead_DW.UDPReceive_NetworkLib[0U], 0, "0.0.0.0",
                        25000, "0.0.0.0", -1, 8192, 4, 0);
    }

    if (*sErr == 0) {
      LibStart(&udpRead_DW.UDPReceive_NetworkLib[0U]);
    }

    if (*sErr != 0) {
      DestroyUDPInterface(&udpRead_DW.UDPReceive_NetworkLib[0U]);
      if (*sErr != 0) {
        rtmSetErrorStatus(udpRead_M, sErr);
        rtmSetStopRequested(udpRead_M, 1);
      }
    }

    /* End of Start for S-Function (sdspFromNetwork): '<Root>/UDP Receive' */

    /* InitializeConditions for UnitDelay: '<S3>/FixPt Unit Delay2' */
    udpRead_DW.FixPtUnitDelay2_DSTATE =
      udpRead_P.FixPtUnitDelay2_InitialConditio;

    /* InitializeConditions for UnitDelay: '<S3>/FixPt Unit Delay1' */
    udpRead_DW.FixPtUnitDelay1_DSTATE = udpRead_P.UnitDelayResettable_vinit;
  }
}
コード例 #19
0
ファイル: ert_main.c プロジェクト: shohei/param-identifier
int_T main(void)
{
  init();

#ifdef _RTT_USE_SERIAL1_

  Serial_begin(1, 9600);

#endif

#ifdef _RTT_USE_SERIAL2_

  Serial_begin(2, 9600);

#endif

#ifdef _RTT_USE_SERIAL3_

  Serial_begin(3, 9600);

#endif

  /* initialize external mode */
  rtParseArgsForExtMode(0, NULL);
  velo_id_gain_initialize();

  /* External mode */
  rtSetTFinalForExtMode(&rtmGetTFinal(velo_id_gain_M));
  rtExtModeCheckInit(2);

  {
    boolean_T rtmStopReq = false;
    rtExtModeWaitForStartPkt(velo_id_gain_M->extModeInfo, 2, &rtmStopReq);
    if (rtmStopReq) {
      rtmSetStopRequested(velo_id_gain_M, true);
    }
  }

  rtERTExtModeStartMsg();
  arduino_Timer_Setup();

  /* The main step loop */
  while ((rtmGetErrorStatus(velo_id_gain_M) == (NULL)) && !rtmGetStopRequested
         (velo_id_gain_M)) {
    /* External mode */
    {
      boolean_T rtmStopReq = false;
      rtExtModeOneStep(velo_id_gain_M->extModeInfo, 2, &rtmStopReq);
      if (rtmStopReq) {
        rtmSetStopRequested(velo_id_gain_M, true);
      }
    }

    rtExtModeCheckEndTrigger();
  }

  rtExtModeShutdown(2);
  delay(1000);
  velo_id_gain_terminate();

  /* Disable Interrupts */
  cli();
  return 0;
}
/* 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 */
}
コード例 #21
0
ファイル: ert_main.c プロジェクト: HSKA-CuBa/CuBa_git
int main(void)
{
  volatile boolean_T runModel = 1;
  float modelBaseRate = 0.01;
  float systemClock = 168;

#ifndef USE_RTX

  __disable_irq();

#endif

  ;
  stm32f4xx_init_board();
  SystemCoreClockUpdate();
  bootloaderInit();
  rtmSetErrorStatus(BeschleunigungsAuswertung_M, 0);

  /* initialize external mode */
  rtParseArgsForExtMode(0, NULL);
  BeschleunigungsAuswertung_initialize();
  __enable_irq();

  /* External mode */
  rtSetTFinalForExtMode(&rtmGetTFinal(BeschleunigungsAuswertung_M));
  rtExtModeCheckInit(1);

  {
    boolean_T rtmStopReq = false;
    rtExtModeWaitForStartPkt(BeschleunigungsAuswertung_M->extModeInfo, 1,
      &rtmStopReq);
    if (rtmStopReq) {
      rtmSetStopRequested(BeschleunigungsAuswertung_M, true);
    }
  }

  rtERTExtModeStartMsg();
  __disable_irq();
  ARMCM_SysTick_Config(modelBaseRate);
  runModel =
    (rtmGetErrorStatus(BeschleunigungsAuswertung_M) == (NULL)) &&
    !rtmGetStopRequested(BeschleunigungsAuswertung_M);
  __enable_irq();
  __enable_irq();
  while (runModel) {
    /* External mode */
    {
      boolean_T rtmStopReq = false;
      rtExtModeOneStep(BeschleunigungsAuswertung_M->extModeInfo, 1, &rtmStopReq);
      if (rtmStopReq) {
        rtmSetStopRequested(BeschleunigungsAuswertung_M, true);
      }
    }

    runModel =
      (rtmGetErrorStatus(BeschleunigungsAuswertung_M) == (NULL)) &&
      !rtmGetStopRequested(BeschleunigungsAuswertung_M);
  }

  rtExtModeShutdown(1);

#ifndef USE_RTX

  (void)systemClock;

#endif

  ;

  /* Disable rt_OneStep() here */

  /* Terminate model */
  BeschleunigungsAuswertung_terminate();
  __disable_irq();
  return 0;
}
コード例 #22
0
ファイル: udpRead.c プロジェクト: gte999s/QuadCopter
/* Model output function */
void udpRead_output(void)
{
  char_T *sErr;
  int32_T samplesRead;
  boolean_T rtb_Compare;
  int32_T s5_iter;
  real_T tmp;
  int32_T exitg1;
  int32_T exitg2;

  /* Reset subsysRan breadcrumbs */
  srClearBC(udpRead_DW.ForIteratorSubsystem_SubsysRanB);

  /* Reset subsysRan breadcrumbs */
  srClearBC(udpRead_DW.ForIteratorSubsystem1_SubsysRan);

  /* Reset subsysRan breadcrumbs */
  srClearBC(udpRead_DW.EnabledSubsystem_SubsysRanBC);

  /* S-Function (sdspFromNetwork): '<Root>/UDP Receive' */
  sErr = GetErrorBuffer(&udpRead_DW.UDPReceive_NetworkLib[0U]);
  samplesRead = 31;
  LibOutputs_Network(&udpRead_DW.UDPReceive_NetworkLib[0U],
                     &udpRead_B.UDPReceive_o1[0U], &samplesRead);
  if (*sErr != 0) {
    rtmSetErrorStatus(udpRead_M, sErr);
    rtmSetStopRequested(udpRead_M, 1);
  }

  udpRead_B.UDPReceive_o2 = (uint16_T)samplesRead;

  /* End of S-Function (sdspFromNetwork): '<Root>/UDP Receive' */

  /* RelationalOperator: '<S1>/Compare' incorporates:
   *  Constant: '<S1>/Constant'
   */
  rtb_Compare = (udpRead_B.UDPReceive_o2 > udpRead_P.Constant_Value_p);

  /* Outputs for Enabled SubSystem: '<Root>/Enabled Subsystem' incorporates:
   *  EnablePort: '<S2>/Enable'
   */
  if (rtb_Compare) {
    if (!udpRead_DW.EnabledSubsystem_MODE) {
      udpRead_DW.EnabledSubsystem_MODE = true;
    }

    /* Outputs for Iterator SubSystem: '<S2>/For Iterator Subsystem1' incorporates:
     *  ForIterator: '<S5>/For Iterator'
     */
    /* Selector: '<S5>/Selector' incorporates:
     *  Selector: '<S5>/Selector1'
     *  Selector: '<S5>/Selector2'
     */
    s5_iter = 24;
    do {
      exitg2 = 0;
      if (udpRead_P.Constant1_Value < 0.0) {
        tmp = ceil(udpRead_P.Constant1_Value);
      } else {
        tmp = floor(udpRead_P.Constant1_Value);
      }

      if (rtIsNaN(tmp) || rtIsInf(tmp)) {
        tmp = 0.0;
      } else {
        tmp = fmod(tmp, 4.294967296E+9);
      }

      if (s5_iter - 23 <= (tmp < 0.0 ? -(int32_T)(uint32_T)-tmp : (int32_T)
                           (uint32_T)tmp)) {
        udpRead_B.Selector = udpRead_B.UDPReceive_o1[s5_iter];
        udpRead_B.Selector1 = udpRead_B.UDPReceive_o1[s5_iter + 2];
        udpRead_B.Selector2 = udpRead_B.UDPReceive_o1[s5_iter + 4];
        srUpdateBC(udpRead_DW.ForIteratorSubsystem1_SubsysRan);
        s5_iter++;
      } else {
        exitg2 = 1;
      }
    } while (exitg2 == 0);

    /* End of Outputs for SubSystem: '<S2>/For Iterator Subsystem1' */

    /* Outputs for Iterator SubSystem: '<S2>/For Iterator Subsystem' incorporates:
     *  ForIterator: '<S4>/For Iterator'
     */
    s5_iter = 1;
    do {
      exitg1 = 0;
      if (udpRead_P.Constant_Value < 0.0) {
        tmp = ceil(udpRead_P.Constant_Value);
      } else {
        tmp = floor(udpRead_P.Constant_Value);
      }

      if (rtIsNaN(tmp) || rtIsInf(tmp)) {
        tmp = 0.0;
      } else {
        tmp = fmod(tmp, 4.294967296E+9);
      }

      if (s5_iter <= (tmp < 0.0 ? -(int32_T)(uint32_T)-tmp : (int32_T)(uint32_T)
                      tmp)) {
        udpRead_B.Selector_d = udpRead_B.UDPReceive_o1[s5_iter - 1];
        udpRead_B.Selector1_f = udpRead_B.UDPReceive_o1[s5_iter + 3];
        udpRead_B.Selector2_f = udpRead_B.UDPReceive_o1[s5_iter + 7];
        udpRead_B.Compare = (s5_iter == udpRead_P.CompareToConstant_const);
        udpRead_B.Selector3 = udpRead_B.UDPReceive_o1[s5_iter + 11];
        udpRead_B.Selector4 = udpRead_B.UDPReceive_o1[s5_iter + 15];
        udpRead_B.Selector5 = udpRead_B.UDPReceive_o1[s5_iter + 19];
        srUpdateBC(udpRead_DW.ForIteratorSubsystem_SubsysRanB);
        s5_iter++;
      } else {
        exitg1 = 1;
      }
    } while (exitg1 == 0);

    /* End of Outputs for SubSystem: '<S2>/For Iterator Subsystem' */
    srUpdateBC(udpRead_DW.EnabledSubsystem_SubsysRanBC);
  } else {
    if (udpRead_DW.EnabledSubsystem_MODE) {
      udpRead_DW.EnabledSubsystem_MODE = false;
    }
  }

  /* End of Outputs for SubSystem: '<Root>/Enabled Subsystem' */

  /* Switch: '<S3>/Init' incorporates:
   *  Constant: '<S3>/Initial Condition'
   *  Logic: '<S3>/FixPt Logical Operator'
   *  UnitDelay: '<S3>/FixPt Unit Delay1'
   *  UnitDelay: '<S3>/FixPt Unit Delay2'
   */
  if (rtb_Compare || (udpRead_DW.FixPtUnitDelay2_DSTATE != 0)) {
    udpRead_B.Init = udpRead_P.UnitDelayResettable_vinit;
  } else {
    udpRead_B.Init = udpRead_DW.FixPtUnitDelay1_DSTATE;
  }

  /* End of Switch: '<S3>/Init' */

  /* Switch: '<S3>/Reset' incorporates:
   *  Constant: '<S3>/Initial Condition'
   *  DataTypeConversion: '<Root>/Data Type Conversion'
   *  Sum: '<Root>/Sum'
   */
  if (rtb_Compare) {
    udpRead_B.Xnew = udpRead_P.UnitDelayResettable_vinit;
  } else {
    udpRead_B.Xnew = 1.0F + udpRead_B.Init;
  }

  /* End of Switch: '<S3>/Reset' */
}