/*
 * File: Vejlupek_Car4_HIL_PIDSpeed.c
 *
 * Real-Time Workshop code generated for Simulink model Vejlupek_Car4_HIL_PIDSpeed.
 *
 * Model version                        : 1.460
 * Real-Time Workshop file version      : 7.3  (R2009a)  15-Jan-2009
 * Real-Time Workshop file generated on : Fri Apr 30 16:12:44 2010
 * TLC version                          : 7.3 (Jan 18 2009)
 * C source code generated on           : Fri Apr 30 16:12:44 2010
 *--------------------------------------------------------------
 *   Embedded Coder for Microchip dsPIC family.                 |
 *   Generate .c and .h files from your Matlab/simulink model   |
 *   and compile the diagram to .hex and .coff file that can be |
 *   downloaded directly into the microcontroller               |
 *                                                              |
 * Licence Accorded to Ing. Robert Grepl, Ph.D.                 |
 *                     Technical University Brno                |
 *                                                              |
 *   Written by Lubin KERHUEL -  http://www.kerhuel.eu          |
 *  - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - |
 *   Version 3.1                               26-Nov-2009      |
 *   For Matlab 7.8             R2009a                          |
 *--------------------------------------------------------------
 */

#include "Vejlupek_Car4_HIL_PIDSpeed.h"
#include "Vejlupek_Car4_HIL_PIDSpeed_private.h"

/* Exported block states */
uint16_T ct_turns;                     /* '<Root>/Encoder_01' */

/* Block signals (auto storage) */
BlockIO_Vejlupek_Car4_HIL_PIDSp Vejlupek_Car4_HIL_PIDSpeed_B;

/* Block states (auto storage) */
D_Work_Vejlupek_Car4_HIL_PIDSpe Vejlupek_Car4_HIL_PIDSpee_DWork;

/* Real-time model */
RT_MODEL_Vejlupek_Car4_HIL_PIDS Vejlupek_Car4_HIL_PIDSpeed_M_;
RT_MODEL_Vejlupek_Car4_HIL_PIDS *Vejlupek_Car4_HIL_PIDSpeed_M =
  &Vejlupek_Car4_HIL_PIDSpeed_M_;
static void rate_monotonic_scheduler(void);
void sMultiWordShl(const uint32_T u1[], int16_T n1, uint16_T n2, uint32_T y[],
                   int16_T n)
{
  int16_T nb;
  int16_T nc;
  int16_T i;
  uint32_T ys;
  uint32_T u1i;
  uint32_T yi;
  uint16_T nr;
  uint16_T nl;
  int16_T loop_ub;
  int16_T loop_ub_0;
  int16_T loop_ub_1;
  nb = (int16_T)(n2 >> 5);
  ys = (u1[n1 - 1] & 2147483648UL) != 0UL ? MAX_uint32_T : 0UL;
  nc = nb > n ? n : nb;
  u1i = 0UL;
  loop_ub = nc - 1;
  for (i = 0; i <= loop_ub; i = i + 1) {
    y[i] = 0UL;
  }

  if (nb < n) {
    nl = n2 - ((uint16_T)nb << 5U);
    nb = n1 + nb;
    if (nb > n) {
      nb = n;
    }

    nb = nb - i;
    if (nl > 0U) {
      nr = 32U - nl;
      loop_ub_0 = nb - 1;
      for (nc = 0; nc <= loop_ub_0; nc = nc + 1) {
        yi = u1i >> nr;
        u1i = u1[nc];
        y[i] = u1i << nl | yi;
        i = i + 1;
      }

      if (i < n) {
        y[i] = u1i >> nr | ys << nl;
        i = i + 1;
      }
    } else {
      loop_ub_1 = nb - 1;
      for (nc = 0; nc <= loop_ub_1; nc = nc + 1) {
        y[i] = u1[nc];
        i = i + 1;
      }
    }
  }

  while (i < n) {
    y[i] = ys;
    i = i + 1;
  }
}

void sLong2MultiWord(int32_T u, uint32_T y[], int16_T n)
{
  uint32_T yi;
  int16_T i;
  int16_T loop_ub;
  y[0] = (uint32_T)u;
  yi = u < 0L ? MAX_uint32_T : 0UL;
  loop_ub = n - 1;
  for (i = 1; i <= loop_ub; i = i + 1) {
    y[i] = yi;
  }
}

void MultiWordAdd(const uint32_T u1[], const uint32_T u2[], uint32_T y[],
                  int16_T n)
{
  uint32_T yi;
  uint32_T u1i;
  uint32_T carry = 0UL;
  int16_T i;
  int16_T loop_ub;
  loop_ub = n - 1;
  for (i = 0; i <= loop_ub; i = i + 1) {
    u1i = u1[i];
    yi = (u1i + u2[i]) + carry;
    y[i] = yi;
    carry = carry != 0UL ? yi <= u1i ? 1UL : 0UL : yi < u1i ? 1UL : 0UL;
  }
}

uint32_T MultiWord2uLong(const uint32_T u[])
{
  uint32_T y;
  y = u[0];
  return y;
}

void sMultiWordShr(const uint32_T u1[], int16_T n1, uint16_T n2, uint32_T y[],
                   int16_T n)
{
  int16_T nb;
  int16_T i;
  uint32_T ys;
  uint32_T yi;
  uint32_T u1i;
  int16_T nc;
  uint16_T nr;
  uint16_T nl;
  int16_T i1;
  int16_T loop_ub;
  int16_T loop_ub_0;
  nb = (int16_T)(n2 >> 5);
  i = 0;
  ys = (u1[n1 - 1] & 2147483648UL) != 0UL ? MAX_uint32_T : 0UL;
  if (nb < n1) {
    nc = n + nb;
    if (nc > n1) {
      nc = n1;
    }

    nr = n2 - ((uint16_T)nb << 5U);
    if (nr > (uint16_T)0) {
      nl = 32U - nr;
      u1i = u1[nb];
      loop_ub = nc - 1;
      for (i1 = nb + 1; i1 <= loop_ub; i1 = i1 + 1) {
        yi = u1i >> nr;
        u1i = u1[i1];
        y[i] = u1i << nl | yi;
        i = i + 1;
      }

      y[i] = (nc < n1 ? u1[nc] : ys) << nl | u1i >> nr;
      i = i + 1;
    } else {
      loop_ub_0 = nc - 1;
      for (i1 = nb; i1 <= loop_ub_0; i1 = i1 + 1) {
        y[i] = u1[i1];
        i = i + 1;
      }
    }
  }

  while (i < n) {
    y[i] = ys;
    i = i + 1;
  }
}

/* Set which subrates need to run this base step (base rate always runs).
 * This function must be called prior to calling the model step function
 * in order to "remember" which rates need to run this base step.  The
 * buffering of events allows for overlapping preemption.
 */
void Vejlupek_Car4_HIL_PIDSpeed_SetEventsForThisBaseStep(boolean_T *eventFlags)
{
  /* Task runs when its counter is zero, computed via rtmStepTask macro */
  eventFlags[1] = ((boolean_T)rtmStepTask(Vejlupek_Car4_HIL_PIDSpeed_M, 1));
}

/* This function updates active task flag for each subrate
 * and rate transition flags for tasks that exchagne data.
 * The function assumes rate-monotonic multitasking scheduler.
 * The function must be called at model base rate so that
 * the generated code self-manages all its subrates and rate
 * transition flags.
 */
static void rate_monotonic_scheduler(void)
{
  /* Compute which subrates run during the next base time step.  Subrates
   * are an integer multiple of the base rate counter.  Therefore, the subtask
   * counter is reset when it reaches its limit (zero means run).
   */
  if (++Vejlupek_Car4_HIL_PIDSpeed_M->Timing.TaskCounters.TID[1] == 10) {/* Sample time: [0.1s, 0.0s] */
    Vejlupek_Car4_HIL_PIDSpeed_M->Timing.TaskCounters.TID[1] = 0;
  }
}

/* Model step function for TID0 */
void Vejlupek_Car4_HIL_PIDSpeed_step0(void) /* Sample time: [0.01s, 0.0s] */
{
  {                                    /* Sample time: [0.01s, 0.0s] */
    rate_monotonic_scheduler();
  }

  {
    real_T rtb_HzLED;

    /* S-Function "dsPIC_Digital_OutputRead" Block: <S6>/Termal_Flag */
    Vejlupek_Car4_HIL_PIDSpeed_B.Termal_Flag = LATCbits.LATC5;

    /* S-Function "ADC" Block: <S6>/proud */
    Vejlupek_Car4_HIL_PIDSpeed_B.proud = ADC1BUF0;

    /* S-Function "dsPIC_Digital_OutputWrite" Block: <S6>/Brake_(odpojeni_napeti) */
    LATAbits.LATA7 = FALSE;

    /* DiscretePulseGenerator: '<S1>/2 Hz LED' */
    rtb_HzLED = (Vejlupek_Car4_HIL_PIDSpee_DWork.clockTickCounter < 50L) &&
      (Vejlupek_Car4_HIL_PIDSpee_DWork.clockTickCounter >= 0L) ? 1.0 : 0.0;
    if (Vejlupek_Car4_HIL_PIDSpee_DWork.clockTickCounter >= 99L) {
      Vejlupek_Car4_HIL_PIDSpee_DWork.clockTickCounter = 0L;
    } else {
      Vejlupek_Car4_HIL_PIDSpee_DWork.clockTickCounter =
        Vejlupek_Car4_HIL_PIDSpee_DWork.clockTickCounter + 1L;
    }

    /* DataTypeConversion: '<S1>/Data Type Conversion' */
    Vejlupek_Car4_HIL_PIDSpeed_B.DataTypeConversion_e = (rtb_HzLED != 0.0);

    /* S-Function "dsPIC_Digital_OutputWrite" Block: <S1>/A8 - GP LED */
    LATAbits.LATA8 = Vejlupek_Car4_HIL_PIDSpeed_B.DataTypeConversion_e;
  }
}

/* Model step function for TID1 */
void Vejlupek_Car4_HIL_PIDSpeed_step1(void) /* Sample time: [0.1s, 0.0s] */
{
  /* local block i/o variables */
  real_T rtb_TSamp;
  int16_T rtb_Sum1_m;

  {
    int64m_T numAccum;
    int32_T k;
    uint8_T rtb_UnitDelay4;
    uint8_T rtb_UnitDelay9;
    uint8_T rtb_UnitDelay7;
    uint8_T rtb_UnitDelay8;
    int16_T rtb_MultiportSwitch;
    real_T rtb_DiscreteTimeIntegrator;
    real_T rtb_Diff;
    uint8_T rtb_UnitDelay1_a;
    uint8_T rtb_UnitDelay2;
    uint8_T rtb_UnitDelay3;
    uint8_T rtb_UnitDelay5;
    uint8_T rtb_UnitDelay6;
    uint16_T rtb_Saturation;
    int32_T rtb_Switch1;
    uint16_T rtb_DiscreteFilter2;
    int16_T rtb_PGain;
    int16_T rtb_Sum1;
    real_T tmp;
    real_T tmp_0;
    int64m_T tmp_1;
    int64m_T tmp_2;
    int64m_T tmp_3;
    int64m_T tmp_4;

    /* S-Function "dsPIC_C_function_Call" Block: <S6>/Angle => Count by 1 roll = 1000 QEI 500 mode x2 without index chx */
    Vejlupek_Car4_HIL_PIDSpeed_B.AngleCountby1roll1000QEI500mode = GetQEI();

    /* Switch: '<S4>/Switch1' incorporates:
     *  Constant: '<S4>/Constant1'
     *  Sum: '<S4>/Sum4'
     *  Sum: '<S4>/Sum5'
     *  UnitDelay: '<S4>/Unit Delay1'
     */
    if ((int32_T)Vejlupek_Car4_HIL_PIDSpeed_B.AngleCountby1roll1000QEI500mode -
        (int32_T)Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay1_DSTATE >= 32768L) {
      rtb_Switch1 = (int32_T)
        Vejlupek_Car4_HIL_PIDSpeed_B.AngleCountby1roll1000QEI500mode - 65535L;
    } else {
      rtb_Switch1 = (int32_T)
        Vejlupek_Car4_HIL_PIDSpeed_B.AngleCountby1roll1000QEI500mode;
    }

    /* DiscreteFilter: '<S4>/Discrete Filter2' */
    Vejlupek_Car4_HIL_PIDSpee_DWork.DiscreteFilter2_tmp = rtb_Switch1;
    sLong2MultiWord(Vejlupek_Car4_HIL_PIDSpee_DWork.DiscreteFilter2_tmp,
                    &tmp_1.chunks[0U], 2);
    sMultiWordShl(&tmp_1.chunks[0U], 2, (uint16_T)30, &numAccum.chunks[0U], 2);
    sLong2MultiWord(Vejlupek_Car4_HIL_PIDSpee_DWork.DiscreteFilter2_DSTATE,
                    &tmp_2.chunks[0U], 2);
    sMultiWordShl(&tmp_2.chunks[0U], 2, (uint16_T)30, &tmp_1.chunks[0U], 2);
    MultiWordAdd(&numAccum.chunks[0U], &tmp_1.chunks[0U], &tmp_2.chunks[0U], 2);
    numAccum = tmp_2;
    sMultiWordShr(&numAccum.chunks[0U], 2, (uint16_T)30, &tmp_1.chunks[0U], 2);
    rtb_DiscreteFilter2 = (uint16_T)MultiWord2uLong(&tmp_1.chunks[0U]);

    /* Switch: '<S4>/Switch' incorporates:
     *  Constant: '<S4>/Constant'
     *  Sum: '<S4>/Sum2'
     *  Sum: '<S4>/Sum3'
     *  UnitDelay: '<S4>/Unit Delay1'
     */
    if ((int32_T)Vejlupek_Car4_HIL_PIDSpeed_B.AngleCountby1roll1000QEI500mode -
        (int32_T)Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay1_DSTATE >= -32768L) {
      rtb_Switch1 = (int32_T)Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay1_DSTATE;
    } else {
      rtb_Switch1 = (int32_T)Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay1_DSTATE -
        65535L;
    }

    /* DiscreteFilter: '<S4>/Discrete Filter1' */
    Vejlupek_Car4_HIL_PIDSpee_DWork.DiscreteFilter1_tmp = rtb_Switch1;
    sLong2MultiWord(Vejlupek_Car4_HIL_PIDSpee_DWork.DiscreteFilter1_tmp,
                    &tmp_3.chunks[0U], 2);
    sMultiWordShl(&tmp_3.chunks[0U], 2, (uint16_T)30, &numAccum.chunks[0U], 2);
    sLong2MultiWord(Vejlupek_Car4_HIL_PIDSpee_DWork.DiscreteFilter1_DSTATE,
                    &tmp_4.chunks[0U], 2);
    sMultiWordShl(&tmp_4.chunks[0U], 2, (uint16_T)30, &tmp_3.chunks[0U], 2);
    MultiWordAdd(&numAccum.chunks[0U], &tmp_3.chunks[0U], &tmp_4.chunks[0U], 2);
    numAccum = tmp_4;
    sMultiWordShr(&numAccum.chunks[0U], 2, (uint16_T)30, &tmp_3.chunks[0U], 2);
    rtb_Saturation = (uint16_T)MultiWord2uLong(&tmp_3.chunks[0U]);

    /* DiscreteFilter: '<Root>/Discrete Filter' incorporates:
     *  Sum: '<S4>/Sum1'
     */
    Vejlupek_Car4_HIL_PIDSpee_DWork.DiscreteFilter_tmp = (int16_T)
      rtb_DiscreteFilter2 - (int16_T)rtb_Saturation;
    rtb_PGain = (int16_T)((((int32_T)
      Vejlupek_Car4_HIL_PIDSpee_DWork.DiscreteFilter_tmp << 14) + ((int32_T)
      Vejlupek_Car4_HIL_PIDSpee_DWork.DiscreteFilter_DSTATE << 14)) >> 15);

    /* DataTypeConversion: '<Root>/Data Type Conversion' incorporates:
     *  Constant: '<Root>/Constant'
     *  Sum: '<Root>/Sum1'
     */
    Vejlupek_Car4_HIL_PIDSpeed_B.DataTypeConversion = (uint16_T)(int16_T)
      ((int32_T)(rtb_PGain >> 1) - 32768L);

    /* S-Function "UART_RX" Block: <Root>/Rx Input1 */
    {
      if (U1STAbits.URXDA != 0)
        _U1RXIF = 1;
      if (RxPtrHaut_Uart1 != RxPtrBas_Uart1) {
        Vejlupek_Car4_HIL_PIDSpeed_B.RxInput1 = (uint8_T)
          RxTab_Uart1[RxPtrBas_Uart1 ++];/* Use only the 8 low bytes or RxReg */
        RxPtrBas_Uart1 &= (Rx_BUFF_SIZE_Uart1-1);
      } else {
        Vejlupek_Car4_HIL_PIDSpeed_B.RxInput1 = 0;
      }
    }

    /* UnitDelay: '<S2>/Unit Delay4' */
    rtb_UnitDelay4 = Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay4_DSTATE;

    /* UnitDelay: '<S2>/Unit Delay9' */
    rtb_UnitDelay9 = Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay9_DSTATE;

    /* UnitDelay: '<S2>/Unit Delay7' */
    rtb_UnitDelay7 = Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay7_DSTATE;

    /* UnitDelay: '<S2>/Unit Delay8' */
    rtb_UnitDelay8 = Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay8_DSTATE;

    /* MultiPortSwitch: '<S14>/Multiport Switch' incorporates:
     *  Constant: '<S10>/Constant'
     *  Constant: '<S2>/Constant3'
     *  Constant: '<S7>/Constant'
     *  Constant: '<S8>/Constant'
     *  Constant: '<S9>/Constant'
     *  DataTypeConversion: '<S2>/Data Type Conversion'
     *  Gain: '<S2>/Gain4'
     *  Logic: '<S2>/Logical Operator'
     *  RelationalOperator: '<S10>/Compare'
     *  RelationalOperator: '<S7>/Compare'
     *  RelationalOperator: '<S8>/Compare'
     *  RelationalOperator: '<S9>/Compare'
     *  Sum: '<S2>/Sum3'
     *  UnitDelay: '<S14>/Memory_1'
     *  UnitDelay: '<S2>/Unit Delay10'
     */
    if (((Vejlupek_Car4_HIL_PIDSpeed_B.RxInput1 == 211) &&
         (Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay4_DSTATE == 255) &&
         (Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay9_DSTATE == 211) &&
         (Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay10_DSTATE == 255)) == 0) {
      rtb_MultiportSwitch = Vejlupek_Car4_HIL_PIDSpee_DWork.Memory_1_DSTATE;
    } else {
      rtb_MultiportSwitch = (int16_T)(((int32_T)
        Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay7_DSTATE - 32768L) + (int32_T)
                                      ((uint16_T)
        Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay8_DSTATE << 8));
    }

    /* Sum: '<Root>/Sum' */
    rtb_Sum1 = rtb_MultiportSwitch - (rtb_PGain >> 1);

    /* Gain: '<S3>/P Gain' */
    rtb_Switch1 = 7291L * (int32_T)rtb_Sum1;
    rtb_PGain = (int16_T)((int32_T)((rtb_Switch1 < 0L) && ((rtb_Switch1 & 1023L)
      != 0L)) + (rtb_Switch1 >> 10));

    /* DiscreteIntegrator: '<S3>/Discrete-Time Integrator' */
    rtb_DiscreteTimeIntegrator =
      Vejlupek_Car4_HIL_PIDSpee_DWork.DiscreteTimeIntegrator_DSTATE;

    /* Gain: '<S3>/D Gain' */
    rtb_Switch1 = 12583L * (int32_T)rtb_Sum1;
    rtb_Sum1_m = ((rtb_Switch1 < 0L) && ((rtb_Switch1 & 131071L) != 0L)) +
      (int16_T)(rtb_Switch1 >> 17);

    /* S-Function (sfix_tsampmath): '<S15>/TSamp' */

    /* Sample Time Math Block: '<S15>/TSamp'
     *
     * y = u * K     where     K = 1 / ( w * Ts )
     *
     * Input0  Data Type:  Fixed Point    S16  2^-5
     * Output0 Data Type:  Floating Point real_T
     *
     * Parameter: WtEt == Weighted Elapsed Time
     *   Data Type:  Fixed Point    S16  2^1
     *
     */
    rtb_TSamp = ldexp((real_T)rtb_Sum1_m, (int16_T)-5L) * ldexp((real_T)5,
      (int16_T)1L);

    /* Sum: '<S15>/Diff' incorporates:
     *  UnitDelay: '<S15>/UD'
     */
    rtb_Diff = rtb_TSamp - Vejlupek_Car4_HIL_PIDSpee_DWork.UD_DSTATE;

    /* Sum: '<S3>/Sum1' */
    if (rtIsNaN(rtb_Diff) || rtIsInf(rtb_Diff)) {
      tmp_0 = 0.0;
    } else {
      tmp_0 = fmod(floor(rtb_Diff), 4.294967296E+009);
    }

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

    rtb_Sum1_m = (int16_T)(((int32_T)(rtb_PGain >> 5) + (tmp < 0.0 ? -((int32_T)
                              (uint32_T)(-tmp)) : (int32_T)(uint32_T)tmp)) +
      (tmp_0 < 0.0 ? -((int32_T)(uint32_T)(-tmp_0)) : (int32_T)(uint32_T)tmp_0));

    /* DataTypeConversion: '<Root>/Data Type Conversion1' incorporates:
     *  Constant: '<Root>/Constant1'
     *  Sum: '<Root>/Sum2'
     */
    Vejlupek_Car4_HIL_PIDSpeed_B.DataTypeConversion1 = (uint16_T)(int16_T)
      ((int32_T)rtb_MultiportSwitch - 32768L);

    /* DataTypeConversion: '<Root>/Data Type Conversion2' incorporates:
     *  Constant: '<Root>/Constant2'
     *  Sum: '<Root>/Sum3'
     */
    Vejlupek_Car4_HIL_PIDSpeed_B.DataTypeConversion2 = (uint16_T)(int16_T)
      ((int32_T)(rtb_PGain >> 5) + 1000L);

    /* DataTypeConversion: '<Root>/Data Type Conversion3' incorporates:
     *  Constant: '<Root>/Constant3'
     *  Sum: '<Root>/Sum4'
     */
    if (rtIsNaN(rtb_DiscreteTimeIntegrator) || rtIsInf
        (rtb_DiscreteTimeIntegrator)) {
      tmp_0 = 0.0;
    } else {
      tmp_0 = fmod(floor(rtb_DiscreteTimeIntegrator), 4.294967296E+009);
    }

    Vejlupek_Car4_HIL_PIDSpeed_B.DataTypeConversion3 = (uint16_T)(int16_T)
      ((tmp_0 < 0.0 ? -((int32_T)(uint32_T)(-tmp_0)) : (int32_T)(uint32_T)tmp_0)
       + 1000L);

    /* DataTypeConversion: '<Root>/Data Type Conversion4' incorporates:
     *  Constant: '<Root>/Constant4'
     *  Sum: '<Root>/Sum5'
     */
    if (rtIsNaN(rtb_Diff) || rtIsInf(rtb_Diff)) {
      tmp_0 = 0.0;
    } else {
      tmp_0 = fmod(floor(rtb_Diff), 4.294967296E+009);
    }

    Vejlupek_Car4_HIL_PIDSpeed_B.DataTypeConversion4 = (uint16_T)(int16_T)
      ((tmp_0 < 0.0 ? -((int32_T)(uint32_T)(-tmp_0)) : (int32_T)(uint32_T)tmp_0)
       + 1000L);

    /* DataTypeConversion: '<Root>/Data Type Conversion5' incorporates:
     *  Constant: '<Root>/Constant5'
     *  Sum: '<Root>/Sum6'
     */
    Vejlupek_Car4_HIL_PIDSpeed_B.DataTypeConversion5 = (uint16_T)(int16_T)(1000L
      + (int32_T)rtb_Sum1_m);

    /* S-Function "dsPIC_UART_TX_Labview_Matlab" Block: <Root>/TX Output Multiplexed for Matlab // Labview */
    {
      static uint16_T nextToSend = 1;
      if ((nextToSend & 1) != 0) {
        if (!send232Buff_Uart1(Vejlupek_Car4_HIL_PIDSpeed_B.DataTypeConversion,1
             + 0U*4) )
          nextToSend = (nextToSend << 1);
      }

      if ((nextToSend & 2) != 0) {
        if (!send232Buff_Uart1(Vejlupek_Car4_HIL_PIDSpeed_B.DataTypeConversion1,
                               1 + 1U*4) )
          nextToSend = (nextToSend << 1);
      }

      if ((nextToSend & 4) != 0) {
        if (!send232Buff_Uart1(Vejlupek_Car4_HIL_PIDSpeed_B.DataTypeConversion2,
                               1 + 2U*4) )
          nextToSend = (nextToSend << 1);
      }

      if ((nextToSend & 8) != 0) {
        if (!send232Buff_Uart1(Vejlupek_Car4_HIL_PIDSpeed_B.DataTypeConversion3,
                               1 + 3U*4) )
          nextToSend = (nextToSend << 1);
      }

      if ((nextToSend & 16) != 0) {
        if (!send232Buff_Uart1(Vejlupek_Car4_HIL_PIDSpeed_B.DataTypeConversion4,
                               1 + 4U*4) )
          nextToSend = (nextToSend << 1);
      }

      if (nextToSend >= 32)            /* should be &, but get robust here */
      {
        if (!send232Buff_Uart1(Vejlupek_Car4_HIL_PIDSpeed_B.DataTypeConversion5,
                               1 + 5U*4) )
          nextToSend = 1;
      }
    }

    /* UnitDelay: '<S2>/Unit Delay1' */
    rtb_UnitDelay1_a = Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay1_DSTATE_g;

    /* UnitDelay: '<S2>/Unit Delay2' */
    rtb_UnitDelay2 = Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay2_DSTATE;

    /* UnitDelay: '<S2>/Unit Delay3' */
    rtb_UnitDelay3 = Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay3_DSTATE;

    /* UnitDelay: '<S2>/Unit Delay5' */
    rtb_UnitDelay5 = Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay5_DSTATE;

    /* UnitDelay: '<S2>/Unit Delay6' */
    rtb_UnitDelay6 = Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay6_DSTATE;

    /* Gain: '<S3>/I Gain' */
    rtb_Switch1 = 9339L * (int32_T)rtb_Sum1;

    /* Saturate: '<S16>/Saturation' incorporates:
     *  Abs: '<S16>/Abs'
     */
    k = (int32_T)rt_ABS(rtb_Sum1_m);

    /* Gain: '<S16>/Gain' */
    Vejlupek_Car4_HIL_PIDSpeed_B.Gain = 40U * (uint16_T)rt_SATURATE(k, 0L, 1000L);

    /* S-Function "dsPIC_PWM_motor" Block: <S16>/PWM Motor Output */
    P1DC1 = Vejlupek_Car4_HIL_PIDSpeed_B.Gain ;

    /* Switch: '<S16>/Switch' incorporates:
     *  Constant: '<S16>/Constant'
     *  Constant: '<S16>/Constant1'
     */
    if (rtb_Sum1_m >= 0) {
      Vejlupek_Car4_HIL_PIDSpeed_B.Switch = true;
    } else {
      Vejlupek_Car4_HIL_PIDSpeed_B.Switch = false;
    }

    /* S-Function "dsPIC_Digital_OutputWrite" Block: <S16>/smer */
    LATCbits.LATC3 = Vejlupek_Car4_HIL_PIDSpeed_B.Switch;

    /* Update for UnitDelay: '<S4>/Unit Delay1' */
    Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay1_DSTATE =
      Vejlupek_Car4_HIL_PIDSpeed_B.AngleCountby1roll1000QEI500mode;

    /* Update for DiscreteFilter: '<S4>/Discrete Filter2' */
    Vejlupek_Car4_HIL_PIDSpee_DWork.DiscreteFilter2_DSTATE =
      Vejlupek_Car4_HIL_PIDSpee_DWork.DiscreteFilter2_tmp;

    /* Update for DiscreteFilter: '<S4>/Discrete Filter1' */
    Vejlupek_Car4_HIL_PIDSpee_DWork.DiscreteFilter1_DSTATE =
      Vejlupek_Car4_HIL_PIDSpee_DWork.DiscreteFilter1_tmp;

    /* Update for DiscreteFilter: '<Root>/Discrete Filter' */
    Vejlupek_Car4_HIL_PIDSpee_DWork.DiscreteFilter_DSTATE =
      Vejlupek_Car4_HIL_PIDSpee_DWork.DiscreteFilter_tmp;

    /* Update for UnitDelay: '<S2>/Unit Delay4' */
    Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay4_DSTATE = rtb_UnitDelay3;

    /* Update for UnitDelay: '<S2>/Unit Delay9' */
    Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay9_DSTATE = rtb_UnitDelay8;

    /* Update for UnitDelay: '<S2>/Unit Delay10' */
    Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay10_DSTATE = rtb_UnitDelay9;

    /* Update for UnitDelay: '<S14>/Memory_1' */
    Vejlupek_Car4_HIL_PIDSpee_DWork.Memory_1_DSTATE = rtb_MultiportSwitch;

    /* Update for UnitDelay: '<S2>/Unit Delay7' */
    Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay7_DSTATE = rtb_UnitDelay6;

    /* Update for UnitDelay: '<S2>/Unit Delay8' */
    Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay8_DSTATE = rtb_UnitDelay7;

    /* Update for DiscreteIntegrator: '<S3>/Discrete-Time Integrator' */
    Vejlupek_Car4_HIL_PIDSpee_DWork.DiscreteTimeIntegrator_DSTATE = (real_T)
      (int16_T)((int32_T)((rtb_Switch1 < 0L) && ((rtb_Switch1 & 4095L) != 0L)) +
                (rtb_Switch1 >> 12)) * 0.03125 +
      Vejlupek_Car4_HIL_PIDSpee_DWork.DiscreteTimeIntegrator_DSTATE;
    if (Vejlupek_Car4_HIL_PIDSpee_DWork.DiscreteTimeIntegrator_DSTATE >= 127.0)
    {
      Vejlupek_Car4_HIL_PIDSpee_DWork.DiscreteTimeIntegrator_DSTATE = 127.0;
    } else {
      if (Vejlupek_Car4_HIL_PIDSpee_DWork.DiscreteTimeIntegrator_DSTATE <=
          -127.0) {
        Vejlupek_Car4_HIL_PIDSpee_DWork.DiscreteTimeIntegrator_DSTATE = -127.0;
      }
    }

    /* Update for UnitDelay: '<S15>/UD' */
    Vejlupek_Car4_HIL_PIDSpee_DWork.UD_DSTATE = rtb_TSamp;

    /* Update for UnitDelay: '<S2>/Unit Delay1' */
    Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay1_DSTATE_g =
      Vejlupek_Car4_HIL_PIDSpeed_B.RxInput1;

    /* Update for UnitDelay: '<S2>/Unit Delay2' */
    Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay2_DSTATE = rtb_UnitDelay1_a;

    /* Update for UnitDelay: '<S2>/Unit Delay3' */
    Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay3_DSTATE = rtb_UnitDelay2;

    /* Update for UnitDelay: '<S2>/Unit Delay5' */
    Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay5_DSTATE = rtb_UnitDelay4;

    /* Update for UnitDelay: '<S2>/Unit Delay6' */
    Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay6_DSTATE = rtb_UnitDelay5;
  }
}

void Vejlupek_Car4_HIL_PIDSpeed_step(int_T tid)
{
  switch (tid) {
   case 0 :
    Vejlupek_Car4_HIL_PIDSpeed_step0();
    break;

   case 1 :
    Vejlupek_Car4_HIL_PIDSpeed_step1();
    break;

   default :
    break;
  }
}

/* Model initialize function */
void Vejlupek_Car4_HIL_PIDSpeed_initialize(boolean_T firstTime)
{
  (void)firstTime;

  /* Registration code */

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

  /* initialize real-time model */
  (void) memset((void *)Vejlupek_Car4_HIL_PIDSpeed_M,0,
                sizeof(RT_MODEL_Vejlupek_Car4_HIL_PIDS));

  /* block I/O */
  (void) memset(((void *) &Vejlupek_Car4_HIL_PIDSpeed_B),0,
                sizeof(BlockIO_Vejlupek_Car4_HIL_PIDSp));

  /* states (dwork) */
  (void) memset((void *)&Vejlupek_Car4_HIL_PIDSpee_DWork, 0,
                sizeof(D_Work_Vejlupek_Car4_HIL_PIDSpe));
  Vejlupek_Car4_HIL_PIDSpee_DWork.DiscreteTimeIntegrator_DSTATE = 0.0;
  Vejlupek_Car4_HIL_PIDSpee_DWork.UD_DSTATE = 0.0;

  /* exported global states */
  ct_turns = 0;

  /* S-Function "dsPIC_MASTER" initialization Block: <Root>/Master */
  /* 1 testfcy 	*/
  /*  2.0E+007	*/
  /* CONFIG TIMER 1 for scheduling steps 	*/
  ConfigIntTimer1(T1_INT_PRIOR_0 & T1_INT_OFF);
  T1CON = 0x8010 ;                     /* T1_PS_1_8	*/
  PR1 = 49999;

  /* Configuration TRIS */
  TRISA = 65151;
  TRISB = 65519;
  TRISC = 65495;

  /* Configuration ADCHS */
  AD1PCFGL = (260U);

  /* S-Function "dsPIC_UART_config" initialization Block: <Root>/UART Configuration */
  /* set priority */
  _U1RXIP = 0x0007 & 2;
  _U1TXIP = 0x0007 & 1;

  /* enable/disable interrupt */
  _U1RXIE = (3 != 0);
  _U1TXIE = 0;
  U1MODE = 0x8080 + 1024;
  U1STA = 0x0 + 1 + 1024 + 192 + (1<<12);
  U1BRG = 64;
  TxPtrHaut_Uart1 = 0;
  TxPtrBas_Uart1 = 0;
  RxPtrHaut_Uart1 = 0;
  RxPtrBas_Uart1 = 0;
  _U1TXIF = 0;

  /* S-Function "dsPIC_ADC" initialization Block: <S6>/proud */
  /* Initialise ADC converter */
  /* 0 mean each 1 conversion */
#define NoSamplesADC                   0
#define ADCSval                        6
#define SAMCval                        (1 << 8)
#define OutFormatBitsval               (0 << 8)
#define VoltRef                        (0 << 13)

  /* Use internal counter Trigger, continuous simulatenous sampling */
  AD1CON1 = 0x0004 | (7 << 5) | (0 << 10)| OutFormatBitsval;
  AD1CON2 = 0x0400 | (NoSamplesADC << 2) | VoltRef;
  AD1CSSL = (32 & 65535);
  AD1CON3 = (ADCSval | SAMCval) & 0xff7f;
  AD1CHS0 = 0x0000;
  IPC2 = (IPC2 | (6 << 12));           /*Interrupt Priority : 6*/
  _AD1IF = 0;

  /* _AD1IE = 1; */
  /* 1 sample/output-Step ; Do not need interrupt so do not enable it. */
  /* Clear Off Any Interrupt Due To Configuration */
  /* Turn on ADC Module */
  AD1CON1bits.ADON= 1;

  /* S-Function "dsPIC_PWM_motor" initialization Block: <S16>/PWM Motor Output */
  /* Initialise Output Capture */
  SetDCMCPWM1(1,0,0);

#define config                         (PWM1_INT_DIS & PWM1_FLTA_DIS_INT & PWM1_INT_PR1 & PWM1_FLTA_INT_PR0)

  ConfigIntMCPWM1( config );

#define config1                        (PWM1_EN & 0xDFFF & PWM1_OP_SCALE1 & PWM1_IPCLK_SCALE1 & PWM1_MOD_FREE)
#define config2                        (0xffff & PWM1_PEN1L & PWM1_MOD1_IND & PWM1_PDIS1H & PWM1_PDIS2L & PWM1_PDIS2H & PWM1_PDIS3L & PWM1_PDIS3H)
#define config3                        (PWM1_SEVOPS1 & PWM1_OSYNC_TCY & PWM1_UEN)

  OpenMCPWM1(20000,0,config1,config2,config3);

  /* Start for DiscretePulseGenerator: '<S1>/2 Hz LED' */
  Vejlupek_Car4_HIL_PIDSpee_DWork.clockTickCounter = 0L;

  /* S-Function "dsPIC_PIN_Mapping : Config" initialization Block: <Root>/PIN Mapping */
  /*unlock */
  OSCCON = 0x46;
  OSCCON = 0x57;
  OSCCONbits.IOLOCK = 0;
  RPINR14 = 2048;
  RPINR18 = 65298;
  RPOR2 = 65283;

  /*lock*/
  OSCCON = 0x46;
  OSCCON = 0x57;
  OSCCONbits.IOLOCK = 1;

  /* S-Function "dsPIC_C_function_Call" Block: <Root>/C Function Call */
  QEIInit();
}

/* File trailer for Real-Time Workshop generated code.
 *
 * [EOF]
 */
