/*
 * Academic License - for use in teaching, academic research, and meeting
 * course requirements at degree granting institutions only.  Not for
 * government, commercial, or other organizational use.
 *
 * File: ADM_Integrated_Logic.c
 *
 * Code generated for Simulink model 'ADM_Integrated_Logic'.
 *
 * Model version                  : 13.55
 * Simulink Coder version         : 24.1 (R2024a) 19-Nov-2023
 * C/C++ source code generated on : Wed May  7 21:12:17 2025
 *
 * Target selection: ert.tlc
 * Embedded hardware selection: NXP->Cortex-M4
 * Code generation objectives:
 *    1. Execution efficiency
 *    2. RAM efficiency
 *    3. Debugging
 * Validation result: Not run
 */

#include "ADM_Integrated_Logic.h"
#include <stdint.h>
#include "ADM_Integrated_Logic_private.h"
#include <math.h>
#include <stdbool.h>

/* Named constants for Chart: '<S7>/Chart' */
#define ADM_Integrated_Logic_IN_HAC_OFF ((uint8_t)1U)
#define ADM_Integrated_Logic_IN_HAC_ON ((uint8_t)2U)

/* Block signals and states (default storage) */
DW_ADM_Integrated_Logic_T ADM_Integrated_Logic_DW;

/* External inputs (root inport signals with default storage) */
ExtU_ADM_Integrated_Logic_T ADM_Integrated_Logic_U;

/* External outputs (root outports fed by signals with default storage) */
ExtY_ADM_Integrated_Logic_T ADM_Integrated_Logic_Y;

/* Real-time model */
static RT_MODEL_ADM_Integrated_Logic_T ADM_Integrated_Logic_M_;
RT_MODEL_ADM_Integrated_Logic_T *const ADM_Integrated_Logic_M =
  &ADM_Integrated_Logic_M_;
static void rate_scheduler(void);

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

/*
 * Output and update for atomic system:
 *    '<S39>/Calculate_F_c'
 *    '<S39>/Calculate_F_c1'
 */
void ADM_Integrated_Lo_Calculate_F_c(double rtu_W, double rtu_theta, double
  *rty_F_c)
{
  /* :  F_c= W*sin(theta); */
  *rty_F_c = rtu_W * sin(rtu_theta);
}

/*
 * Output and update for atomic system:
 *    '<S1>/MATLAB Function1'
 *    '<S1>/MATLAB Function5'
 */
void ADM_Integrated__MATLABFunction1(double rtu_u, double *rty_y)
{
  *rty_y = rtu_u;

  /* :  if u < 140 */
  if (rtu_u < 140.0) {
    /* :  u = 140; */
    *rty_y = 140.0;
  }

  /* :  y = u; */
}

/*
 * Output and update for atomic system:
 *    '<S1>/MATLAB Function2'
 *    '<S1>/MATLAB Function6'
 */
void ADM_Integrated__MATLABFunction2(double rtu_u, double *rty_y)
{
  *rty_y = rtu_u;

  /* :  if u > -140 */
  if (rtu_u > -140.0) {
    /* :  u = -140; */
    *rty_y = -140.0;
  }

  /* :  y = u; */
}

/* Model step function */
void ADM_Integrated_Logic_step(void)
{
  double rtb_Add1;
  double rtb_Add1_tmp;
  double rtb_Add3;
  double rtb_Add_e;
  double rtb_Brake_Torque_Cmd;
  double rtb_Desired_Torque;
  double rtb_Error_m;
  double rtb_Gain_c;
  double rtb_Integrator_1;
  double rtb_Product1_e;
  double rtb_Product1_f;
  double rtb_Product1_iz;
  double rtb_Product1_m;
  double rtb_Product1_pd;
  double rtb_Sum1_aj;
  double rtb_Sum1_i2;
  double rtb_Sum1_lm;
  double rtb_Sum1_o1;
  double rtb_Sum1_p;
  double rtb_Sum_f;
  double rtb_Switch2;
  double rtb_UkYk1;
  double rtb_UkYk1_j;
  double rtb_Vx_Cmd_R;
  double rtb_Yk1_e;
  double rtb_Yk1_p;
  double rtb_deltafalllimit_m;
  double rtb_output_a;
  double rtb_output_p;
  double rtb_y_l;
  int32_t temp_gear;
  bool rtb_Compare;
  bool rtb_Compare_d;

  /* RelationalOperator: '<S2>/Compare' incorporates:
   *  Constant: '<S2>/Constant'
   *  Inport: '<Root>/GV_Operation_Mode'
   */
  rtb_Compare = (ADM_Integrated_Logic_U.GV_Operation_Mode == 2.0);
  if (ADM_Integrated_Logic_M->Timing.TaskCounters.TID[1] == 0) {
    /* Step: '<S45>/Step' incorporates:
     *  DigitalClock: '<S45>/Digital Clock'
     */
    rtb_Add_e = ((ADM_Integrated_Logic_M->Timing.clockTick1) * 1.0);
    if (rtb_Add_e < 0.0) {
      temp_gear = 0;
    } else {
      temp_gear = 1000;
    }

    /* Product: '<S45>/Product' incorporates:
     *  Step: '<S45>/Step'
     */
    rtb_Integrator_1 = (double)temp_gear * rtb_Add_e;

    /* Saturate: '<S7>/Saturation' */
    if (rtb_Integrator_1 > 1000.0) {
      /* Saturate: '<S7>/Saturation' */
      ADM_Integrated_Logic_DW.Saturation = 1000.0;
    } else if (rtb_Integrator_1 < 0.0) {
      /* Saturate: '<S7>/Saturation' */
      ADM_Integrated_Logic_DW.Saturation = 0.0;
    } else {
      /* Saturate: '<S7>/Saturation' */
      ADM_Integrated_Logic_DW.Saturation = rtb_Integrator_1;
    }

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

  /* Product: '<S46>/delta rise limit' incorporates:
   *  Constant: '<S7>/Upper_Torq'
   *  SampleTimeMath: '<S46>/sample time'
   *
   * About '<S46>/sample time':
   *  y = K where K = ( w * Ts )
   *   */
  rtb_Switch2 = 0.016;

  /* UnitDelay: '<S47>/Delay Input2'
   *
   * Block description for '<S47>/Delay Input2':
   *
   *  Store in Global RAM
   */
  rtb_Yk1_e = ADM_Integrated_Logic_DW.DelayInput2_DSTATE_m;

  /* Product: '<S47>/delta rise limit' incorporates:
   *  SampleTimeMath: '<S47>/sample time'
   *
   * About '<S47>/sample time':
   *  y = K where K = ( w * Ts )
   *   */
  rtb_Integrator_1 = 0.002;

  /* Saturate: '<S7>/Pitch_Saturation' incorporates:
   *  DiscreteIntegrator: '<S7>/Integrator_2'
   */
  if (ADM_Integrated_Logic_DW.Integrator_2_DSTATE > 10.0) {
    rtb_Add_e = 10.0;
  } else if (ADM_Integrated_Logic_DW.Integrator_2_DSTATE < -10.0) {
    rtb_Add_e = -10.0;
  } else {
    rtb_Add_e = ADM_Integrated_Logic_DW.Integrator_2_DSTATE;
  }

  /* Sum: '<S47>/Difference Inputs1' incorporates:
   *  Saturate: '<S7>/Pitch_Saturation'
   *
   * Block description for '<S47>/Difference Inputs1':
   *
   *  Add in CPU
   */
  rtb_UkYk1 = rtb_Add_e - rtb_Yk1_e;

  /* Switch: '<S52>/Switch2' incorporates:
   *  RelationalOperator: '<S52>/LowerRelop1'
   */
  if (rtb_UkYk1 <= 0.002) {
    /* Switch: '<S52>/Switch' incorporates:
     *  RelationalOperator: '<S52>/UpperRelop'
     */
    if (rtb_UkYk1 < -0.002) {
      rtb_Integrator_1 = -0.002;
    } else {
      rtb_Integrator_1 = rtb_UkYk1;
    }

    /* End of Switch: '<S52>/Switch' */
  }

  /* End of Switch: '<S52>/Switch2' */

  /* Sum: '<S47>/Difference Inputs2'
   *
   * Block description for '<S47>/Difference Inputs2':
   *
   *  Add in CPU
   */
  rtb_Yk1_e += rtb_Integrator_1;

  /* MATLAB Function: '<S39>/Calculate_F_c' incorporates:
   *  Gain: '<S39>/Gain'
   */
  ADM_Integrated_Lo_Calculate_F_c(ADM_Integrated_Logic_ConstB.W_value,
    0.017453292519943295 * rtb_Yk1_e, &rtb_UkYk1);

  /* Gain: '<S39>/Gain2' incorporates:
   *  Constant: '<S39>/Radius'
   *  Product: '<S39>/Multiply3'
   *  Sum: '<S39>/Required_Brake_Cal'
   */
  /* :  F_R= C_r*W*cos(theta); */
  rtb_Integrator_1 = rtb_UkYk1 * 0.292 * 0.083822296730930432;

  /* Saturate: '<S39>/Saturation' */
  if (rtb_Integrator_1 > 60.0) {
    rtb_Integrator_1 = 60.0;
  } else if (rtb_Integrator_1 < 0.0) {
    rtb_Integrator_1 = 0.0;
  }

  /* Sum: '<S46>/Difference Inputs1' incorporates:
   *  Saturate: '<S39>/Saturation'
   *  UnitDelay: '<S46>/Delay Input2'
   *
   * Block description for '<S46>/Difference Inputs1':
   *
   *  Add in CPU
   *
   * Block description for '<S46>/Delay Input2':
   *
   *  Store in Global RAM
   */
  rtb_UkYk1 = rtb_Integrator_1 - ADM_Integrated_Logic_DW.DelayInput2_DSTATE;

  /* Switch: '<S51>/Switch2' incorporates:
   *  RelationalOperator: '<S51>/LowerRelop1'
   */
  if (rtb_UkYk1 <= 0.016) {
    /* Switch: '<S51>/Switch' incorporates:
     *  RelationalOperator: '<S51>/UpperRelop'
     */
    if (rtb_UkYk1 < -0.16) {
      rtb_Switch2 = -0.16;
    } else {
      rtb_Switch2 = rtb_UkYk1;
    }

    /* End of Switch: '<S51>/Switch' */
  }

  /* End of Switch: '<S51>/Switch2' */

  /* Sum: '<S46>/Difference Inputs2' incorporates:
   *  UnitDelay: '<S46>/Delay Input2'
   *
   * Block description for '<S46>/Difference Inputs2':
   *
   *  Add in CPU
   *
   * Block description for '<S46>/Delay Input2':
   *
   *  Store in Global RAM
   */
  rtb_UkYk1 = rtb_Switch2 + ADM_Integrated_Logic_DW.DelayInput2_DSTATE;

  /* Gain: '<S7>/Grade_GAIN' */
  rtb_Desired_Torque = 0.8 * rtb_UkYk1;

  /* MATLAB Function: '<S14>/Vx_OutPut_Function' incorporates:
   *  Constant: '<S55>/Constant'
   *  Inport: '<Root>/GV_BrakeTorqueCommand'
   *  Inport: '<Root>/GV_Vx_Command'
   *  Inport: '<Root>/GV_Vx_Limit'
   *  RelationalOperator: '<S55>/Compare'
   */
  /* :  if Brake_Input == 0 */
  if (ADM_Integrated_Logic_U.GV_BrakeTorqueCommand < 50.0) {
    /* :  if Vx_Limit <= Vx_Kph */
    if (ADM_Integrated_Logic_U.GV_Vx_Limit <=
        ADM_Integrated_Logic_U.GV_Vx_Command) {
      /* :  Vx_Output = Vx_Limit; */
      rtb_deltafalllimit_m = ADM_Integrated_Logic_U.GV_Vx_Limit;
    } else {
      /* :  else */
      /* :  Vx_Output = Vx_Kph; */
      rtb_deltafalllimit_m = ADM_Integrated_Logic_U.GV_Vx_Command;
    }
  } else {
    /* :  else */
    /* :  Vx_Output = 0; */
    rtb_deltafalllimit_m = 0.0;
  }

  /* End of MATLAB Function: '<S14>/Vx_OutPut_Function' */

  /* RateLimiter: '<S1>/Input_Vx_RateLimiter' */
  rtb_output_a = rtb_deltafalllimit_m - ADM_Integrated_Logic_DW.PrevY;
  if (rtb_output_a > 0.004) {
    rtb_Vx_Cmd_R = ADM_Integrated_Logic_DW.PrevY + 0.004;
  } else if (rtb_output_a < -0.008) {
    rtb_Vx_Cmd_R = ADM_Integrated_Logic_DW.PrevY - 0.008;
  } else {
    rtb_Vx_Cmd_R = rtb_deltafalllimit_m;
  }

  ADM_Integrated_Logic_DW.PrevY = rtb_Vx_Cmd_R;

  /* End of RateLimiter: '<S1>/Input_Vx_RateLimiter' */

  /* RelationalOperator: '<S35>/Compare' incorporates:
   *  Constant: '<S35>/Constant'
   *  Inport: '<Root>/GV_BrakeTorqueCommand'
   */
  rtb_Compare_d = (ADM_Integrated_Logic_U.GV_BrakeTorqueCommand >= 100.0);

  /* Delay: '<S6>/Memory' */
  rtb_deltafalllimit_m = ADM_Integrated_Logic_DW.Memory_DSTATE;

  /* MATLAB Function: '<S6>/Gear_FUNCTION1' incorporates:
   *  Inport: '<Root>/GV_VCU_GearSelStat'
   *  Inport: '<Root>/GV_Vx_Fbk'
   */
  /* :  SAFE_SPEED_THRESHOLD = 0; */
  /*  ¾ÈÀü ±â¾î º¯°æ ¼Óµµ(kph) */
  /*  ±âº»°ª: ÇöÀç ±â¾î À¯Áö */
  /* :  output = Current_Gear; */
  /*   1. P´Ü ¿äûÀÌ ÀÖÀ» °æ¿ì: ¸ðµç »óÅ¿¡¼­ ¿ì¼± ó¸® */
  /* :  if Gear_Cmd == 0 */
  if (ADM_Integrated_Logic_U.GV_VCU_GearSelStat == 0.0) {
    /* :  if Vehicle_Speed <= SAFE_SPEED_THRESHOLD && Brake_Torque_Cmd == 1 */
    if ((ADM_Integrated_Logic_U.GV_Vx_Fbk <= 0.0) && rtb_Compare_d) {
      /* :  output = 0; */
      rtb_deltafalllimit_m = 0.0;

      /*  P´Ü ÁøÀÔ Çã¿ë */
    } else {
      /* :  else */
      /* :  output = Current_Gear; */
      /*  Á¶°Ç ºÒÃæÁ· ½Ã À¯Áö */
    }
  } else {
    /*   2. ÇöÀç ±â¾î »óÅ¿¡ µû¶ó ó¸® */
    /* :  switch Current_Gear */
    switch ((int32_t)rtb_deltafalllimit_m) {
     case 0:
      /* :  case 0 */
      /*  P »óÅÂ */
      /* :  if Vehicle_Speed <= SAFE_SPEED_THRESHOLD */
      if ((ADM_Integrated_Logic_U.GV_Vx_Fbk <= 0.0) &&
          ((ADM_Integrated_Logic_U.GV_VCU_GearSelStat == 1.0) ||
           (ADM_Integrated_Logic_U.GV_VCU_GearSelStat == 3.0) ||
           (ADM_Integrated_Logic_U.GV_VCU_GearSelStat == 2.0))) {
        /* :  if Gear_Cmd == 1 || Gear_Cmd == 3 || Gear_Cmd == 2 */
        /* :  output = 2; */
        rtb_deltafalllimit_m = 2.0;

        /*  P -> N (¸ðµç °æ¿ì NÀ» °ÅÃÄ¾ß ÇϹǷÎ) */
      }
      break;

     case 1:
      /* :  case 1 */
      /*  R »óÅÂ */
      /* :  if Gear_Cmd == 2 */
      if (ADM_Integrated_Logic_U.GV_VCU_GearSelStat == 2.0) {
        /* :  output = 2; */
        rtb_deltafalllimit_m = 2.0;

        /*  R -> N Çã¿ë */
      }
      break;

     case 2:
      /* :  case 2 */
      /*  N »óÅÂ */
      /* :  if Vehicle_Speed <= SAFE_SPEED_THRESHOLD && Brake_Torque_Cmd == 1 */
      if ((ADM_Integrated_Logic_U.GV_Vx_Fbk <= 0.0) && rtb_Compare_d) {
        /* :  if Gear_Cmd == 1 */
        if (ADM_Integrated_Logic_U.GV_VCU_GearSelStat == 1.0) {
          /* :  output = 1; */
          rtb_deltafalllimit_m = 1.0;

          /*  N -> R */
        } else if (ADM_Integrated_Logic_U.GV_VCU_GearSelStat == 3.0) {
          /* :  elseif Gear_Cmd == 3 */
          /* :  output = 3; */
          rtb_deltafalllimit_m = 3.0;

          /*  N -> D */
        }
      }
      break;

     case 3:
      /* :  case 3 */
      /*  D »óÅÂ */
      /* :  if Gear_Cmd == 2 */
      if (ADM_Integrated_Logic_U.GV_VCU_GearSelStat == 2.0) {
        /* :  output = 2; */
        rtb_deltafalllimit_m = 2.0;

        /*  D -> N Çã¿ë */
      }
      break;
    }
  }

  /* End of MATLAB Function: '<S6>/Gear_FUNCTION1' */

  /* Chart: '<S7>/Chart' incorporates:
   *  Constant: '<S7>/Constant'
   *  Constant: '<S7>/Constant1'
   *  Inport: '<Root>/GV_MCU_RPM'
   */
  if (ADM_Integrated_Logic_DW.is_active_c6_ADM_Integrated_Log == 0U) {
    ADM_Integrated_Logic_DW.is_active_c6_ADM_Integrated_Log = 1U;
    ADM_Integrated_Logic_DW.is_c6_ADM_Integrated_Logic =
      ADM_Integrated_Logic_IN_HAC_OFF;

    /* State HAC OFF */
  } else if (ADM_Integrated_Logic_DW.is_c6_ADM_Integrated_Logic ==
             ADM_Integrated_Logic_IN_HAC_OFF) {
    /* :  sf_internal_predicateOutput = RPM_Input < rpm_threshold_low; */
    if (ADM_Integrated_Logic_U.GV_MCU_RPM < -50.0) {
      ADM_Integrated_Logic_DW.is_c6_ADM_Integrated_Logic =
        ADM_Integrated_Logic_IN_HAC_ON;

      /*  State HAC ON */
    } else {
      /* Outport: '<Root>/Debug_HAC_RPM_Decision' */
      /* :  HAC_Output = 0; */
      ADM_Integrated_Logic_Y.Debug_HAC_RPM_Decision = 0.0;
    }

    /* case IN_HAC_ON: */
    /* :  sf_internal_predicateOutput = RPM_Input > rpm_threshold_high; */
  } else if (ADM_Integrated_Logic_U.GV_MCU_RPM > 150.0) {
    ADM_Integrated_Logic_DW.is_c6_ADM_Integrated_Logic =
      ADM_Integrated_Logic_IN_HAC_OFF;

    /* State HAC OFF */
  } else {
    /* Outport: '<Root>/Debug_HAC_RPM_Decision' */
    /* :  HAC_Output = 1; */
    ADM_Integrated_Logic_Y.Debug_HAC_RPM_Decision = 1.0;
  }

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

  /* MATLAB Function: '<S7>/HAC_OFF_OK_Func' */
  /*  Input Parameters: */
  /*  Desired_Brake_Torque_Cmd: Commanded brake torque when HAC is ON */
  /*  Desired_Torque: Required torque at the slope */
  /*  Feedback_Torque: Torque output from the motor */
  /*  HAC_Flag: Current HAC state flag */
  /*  Output Parameters: */
  /*  Brake_Torque_Cmd: Actual brake torque command */
  /*  HAC_ON_OFF_CHECK: Status of HAC (ON or OFF) */
  /*  Persistent variable to track if HAC has ever been ON */
  /*  Initialize HAC_ON_FLAG if it is empty */
  /* :  if isempty(HAC_ON_FLAG) */
  /*  Persistent variable for torque smoothing logic */
  /* :  if isempty(Smoothed_Torque) */
  /*  Persistent variable to store HAC ON Desired Torque */
  /* :  if isempty(HAC_Desired_Torque) */
  /*  Persistent variable to track HAC ON time */
  /* :  if isempty(HAC_ON_Timer) */
  /*  Initialize outputs */
  /* :  Brake_Torque_Cmd = 0; */
  rtb_Brake_Torque_Cmd = 0.0;

  /* Outport: '<Root>/Debug_HAC_FLAG' incorporates:
   *  MATLAB Function: '<S7>/HAC_OFF_OK_Func'
   */
  /* :  HAC_ON_OFF_CHECK = 0; */
  ADM_Integrated_Logic_Y.Debug_HAC_FLAG = 0.0;

  /* MATLAB Function: '<S7>/HAC_OFF_OK_Func' incorporates:
   *  Constant: '<S7>/Accel_Cmd '
   *  Constant: '<S7>/Brake_Cmd'
   *  Constant: '<S7>/Gear_D'
   *  Inport: '<Root>/GV_BrakeTorqueCommand'
   *  Inport: '<Root>/GV_MCU_EstTrq'
   *  Outport: '<Root>/Debug_HAC_RPM_Decision'
   *  RelationalOperator: '<S7>/Relational Operator'
   *  RelationalOperator: '<S7>/Relational Operator1'
   *  RelationalOperator: '<S7>/Relational Operator2'
   *  Sum: '<S7>/HAC_Flags_Add'
   */
  /*  Check if HAC is ON (HAC_Flag >= 4) */
  /* :  if HAC_Flag >= 4 */
  if ((double)(((ADM_Integrated_Logic_U.GV_BrakeTorqueCommand <= 20.0) +
                (rtb_Vx_Cmd_R <= 2.0)) + (rtb_deltafalllimit_m == 3.0)) +
      ADM_Integrated_Logic_Y.Debug_HAC_RPM_Decision >= 4.0) {
    /* :  if HAC_ON_FLAG == 0 */
    if (ADM_Integrated_Logic_DW.HAC_ON_FLAG == 0.0) {
      /* :  HAC_ON_Timer = 0; */
      ADM_Integrated_Logic_DW.HAC_ON_Timer = 0.0;

      /*  Reset timer when HAC turns ON */
    }

    /* :  HAC_ON_FLAG = 1; */
    ADM_Integrated_Logic_DW.HAC_ON_FLAG = 1.0;

    /* :  Brake_Torque_Cmd = Desired_Brake_Torque_Cmd; */
    rtb_Brake_Torque_Cmd = ADM_Integrated_Logic_DW.Saturation;

    /* :  Smoothed_Torque = Desired_Brake_Torque_Cmd; */
    ADM_Integrated_Logic_DW.Smoothed_Torque = ADM_Integrated_Logic_DW.Saturation;

    /* :  if HAC_ON_Timer < 3 */
    if (ADM_Integrated_Logic_DW.HAC_ON_Timer < 3.0) {
      /*  Update Desired Torque for the first 3 seconds */
      /* :  HAC_Desired_Torque = Desired_Torque; */
      ADM_Integrated_Logic_DW.HAC_Desired_Torque = rtb_Desired_Torque;

      /* :  HAC_ON_Timer = HAC_ON_Timer + 1/200; */
      ADM_Integrated_Logic_DW.HAC_ON_Timer += 0.005;

      /*  Assuming 200 Hz update rate */
    }

    /* Outport: '<Root>/Debug_HAC_FLAG' */
    /* :  HAC_ON_OFF_CHECK = 1; */
    ADM_Integrated_Logic_Y.Debug_HAC_FLAG = 1.0;

    /* :  else */
    /*  If HAC has been ON and HAC_Flag drops below 4 */
    /* :  if HAC_ON_FLAG == 1 */
  } else if (ADM_Integrated_Logic_DW.HAC_ON_FLAG == 1.0) {
    /*  Check if Feedback Torque exceeds Desired Torque */
    /* :  if Feedback_Torque >= HAC_Desired_Torque */
    if (ADM_Integrated_Logic_U.GV_MCU_EstTrq >=
        ADM_Integrated_Logic_DW.HAC_Desired_Torque) {
      /* :  HAC_ON_OFF_CHECK = 0; */
      /*  Apply smoothing logic to reduce torque gradually */
      /* :  Smoothed_Torque = Smoothed_Torque - 0.05 * Smoothed_Torque; */
      ADM_Integrated_Logic_DW.Smoothed_Torque -= 0.05 *
        ADM_Integrated_Logic_DW.Smoothed_Torque;

      /*  Adjust smoothing factor as needed */
      /* :  if Smoothed_Torque < 0.01 */
      if (ADM_Integrated_Logic_DW.Smoothed_Torque < 0.01) {
        /*  Threshold to consider torque as zero */
        /* :  Smoothed_Torque = 0; */
        ADM_Integrated_Logic_DW.Smoothed_Torque = 0.0;

        /* :  HAC_ON_FLAG = 0; */
        ADM_Integrated_Logic_DW.HAC_ON_FLAG = 0.0;

        /*  Reset HAC_ON_FLAG if fully off */
        /* :  HAC_Desired_Torque = 0; */
        ADM_Integrated_Logic_DW.HAC_Desired_Torque = 0.0;

        /*  Reset Desired Torque when HAC is OFF */
        /* :  HAC_ON_Timer = 0; */
        ADM_Integrated_Logic_DW.HAC_ON_Timer = 0.0;

        /*  Reset Timer */
      }

      /* :  Brake_Torque_Cmd = Smoothed_Torque; */
      rtb_Brake_Torque_Cmd = ADM_Integrated_Logic_DW.Smoothed_Torque;
    } else {
      /* :  else */
      /*  Keep the brake torque command if torque condition is not met */
      /* :  Brake_Torque_Cmd = Desired_Brake_Torque_Cmd; */
      rtb_Brake_Torque_Cmd = ADM_Integrated_Logic_DW.Saturation;

      /* :  Smoothed_Torque = Desired_Brake_Torque_Cmd; */
      ADM_Integrated_Logic_DW.Smoothed_Torque =
        ADM_Integrated_Logic_DW.Saturation;

      /* Outport: '<Root>/Debug_HAC_FLAG' */
      /* :  HAC_ON_OFF_CHECK = 1; */
      ADM_Integrated_Logic_Y.Debug_HAC_FLAG = 1.0;
    }
  }

  /* MATLAB Function: '<S3>/GearCondition_Brake' */
  /* :  if data == 1 */
  if (rtb_deltafalllimit_m == 1.0) {
    /* :  output = -1; */
    rtb_output_a = -1.0;
  } else if (rtb_deltafalllimit_m == 3.0) {
    /* :  elseif data == 3 */
    /* :  output =  1; */
    rtb_output_a = 1.0;
  } else {
    /* :  else */
    /* :  output = 0; */
    rtb_output_a = 0.0;
  }

  /* End of MATLAB Function: '<S3>/GearCondition_Brake' */

  /* Gain: '<S3>/Brake_GAIN' incorporates:
   *  DiscreteTransferFcn: '<S1>/Discrete Transfer Fcn'
   */
  rtb_Integrator_1 = 0.0625 * ADM_Integrated_Logic_DW.DiscreteTransferFcn_states
    * -80.0;

  /* Saturate: '<S3>/Saturation' */
  if (rtb_Integrator_1 > 1000.0) {
    rtb_Integrator_1 = 1000.0;
  } else if (rtb_Integrator_1 < 0.0) {
    rtb_Integrator_1 = 0.0;
  }

  /* Product: '<S3>/Multiply2' incorporates:
   *  Saturate: '<S3>/Saturation'
   */
  rtb_Integrator_1 *= rtb_output_a;

  /* RateLimiter: '<S3>/Brake_Out_RateLimiter' */
  rtb_output_a = rtb_Integrator_1 - ADM_Integrated_Logic_DW.PrevY_o;
  if (rtb_output_a > 3.0) {
    rtb_Integrator_1 = ADM_Integrated_Logic_DW.PrevY_o + 3.0;
  } else if (rtb_output_a < -3.0) {
    rtb_Integrator_1 = ADM_Integrated_Logic_DW.PrevY_o - 3.0;
  }

  ADM_Integrated_Logic_DW.PrevY_o = rtb_Integrator_1;

  /* End of RateLimiter: '<S3>/Brake_Out_RateLimiter' */

  /* Saturate: '<S3>/Brake_Saturation' */
  if (rtb_Integrator_1 > 1000.0) {
    rtb_Integrator_1 = 1000.0;
  } else if (rtb_Integrator_1 < 0.0) {
    rtb_Integrator_1 = 0.0;
  }

  /* End of Saturate: '<S3>/Brake_Saturation' */

  /* MATLAB Function: '<S1>/Emergency_Brake_Func' incorporates:
   *  Inport: '<Root>/GV_Vx_Fbk'
   *  Sum: '<S1>/Add'
   */
  /* :  if Emergency_Flag == 1 */
  if (rtb_Compare) {
    /* :  Emergency_Brake_Output = Vehicle_Speed*100; */
    rtb_output_a = ADM_Integrated_Logic_U.GV_Vx_Fbk * 100.0;
  } else {
    /* :  else */
    /* :  Emergency_Brake_Output = Brake_Torque_Cmd; */
    rtb_output_a = rtb_Brake_Torque_Cmd + rtb_Integrator_1;
  }

  /* End of MATLAB Function: '<S1>/Emergency_Brake_Func' */

  /* Saturate: '<S1>/Brake_Saturation' */
  if (rtb_output_a > 1500.0) {
    /* Outport: '<Root>/GV_Brake_Command' */
    ADM_Integrated_Logic_Y.GV_Brake_Command = 1500.0;
  } else if (rtb_output_a < 0.0) {
    /* Outport: '<Root>/GV_Brake_Command' */
    ADM_Integrated_Logic_Y.GV_Brake_Command = 0.0;
  } else {
    /* Outport: '<Root>/GV_Brake_Command' */
    ADM_Integrated_Logic_Y.GV_Brake_Command = rtb_output_a;
  }

  /* End of Saturate: '<S1>/Brake_Saturation' */

  /* Outport: '<Root>/Debug_CC_Brake_Output' */
  ADM_Integrated_Logic_Y.Debug_CC_Brake_Output = rtb_Integrator_1;

  /* Outport: '<Root>/Debug_HAC_Brake_Output' */
  ADM_Integrated_Logic_Y.Debug_HAC_Brake_Output = rtb_Brake_Torque_Cmd;

  /* MATLAB Function: '<S3>/Gear_pos_out' incorporates:
   *  MATLAB Function: '<S3>/Target_RPM'
   */
  /* Gear R ´Ü¿¡¼­ - target rpm °ª  */
  /* :  if data == 0 */
  if (rtb_deltafalllimit_m == 0.0) {
    /* :  temp_gear = 0; */
    rtb_output_p = 0.0;

    /* :  temp_gear = 0; */
    temp_gear = 0;
  } else {
    if (rtb_deltafalllimit_m == 2.0) {
      /* :  elseif data == 2 */
      /* :  temp_gear = 0; */
      rtb_output_p = 0.0;
    } else if (rtb_deltafalllimit_m == 1.0) {
      /* :  elseif data == 1 */
      /* :  temp_gear = -1; */
      rtb_output_p = -1.0;
    } else if (rtb_deltafalllimit_m == 3.0) {
      /* :  elseif data == 3 */
      /* :  temp_gear = 1; */
      rtb_output_p = 1.0;
    } else {
      /* :  else */
      /* :  temp_gear = 0; */
      rtb_output_p = 0.0;
    }

    if (rtb_deltafalllimit_m == 2.0) {
      /* :  elseif data == 2 */
      /* :  temp_gear = 0; */
      temp_gear = 0;
    } else if (rtb_deltafalllimit_m == 1.0) {
      /* :  elseif data == 1 */
      /* :  temp_gear = -1; */
      temp_gear = -1;
    } else if (rtb_deltafalllimit_m == 3.0) {
      /* :  elseif data == 3 */
      /* :  temp_gear = 1; */
      temp_gear = 1;
    } else {
      /* :  else */
      /* :  temp_gear = 0; */
      temp_gear = 0;
    }
  }

  /* End of MATLAB Function: '<S3>/Gear_pos_out' */

  /* MATLAB Function: '<S3>/Target_RPM' */
  /* :  output = temp_gear; */
  /* :  gear_ratio = 11.93; */
  /* :  Radius = 0.270781; */
  /* :  engine_eff = 1; */
  /* Gear R ´Ü¿¡¼­ - target rpm °ª  */
  /* :  if data == 0 */
  /* :  speed_m_per_s = Vx_Kph * 1000/3600; */
  /* :  Target_RPM = (speed_m_per_s * gear_ratio * 60) / (Radius*2*pi)*engine_eff*temp_gear; */
  rtb_Brake_Torque_Cmd = rtb_Vx_Cmd_R * 1000.0 / 3600.0 * 11.93 * 60.0 /
    1.7013672006633955 * (double)temp_gear;

  /* RateLimiter: '<S3>/TargetSpd_RateLimiter' */
  rtb_output_a = rtb_Brake_Torque_Cmd - ADM_Integrated_Logic_DW.PrevY_a;
  if (rtb_output_a > 0.4) {
    rtb_output_a = ADM_Integrated_Logic_DW.PrevY_a + 0.4;
  } else if (rtb_output_a < -0.8) {
    rtb_output_a = ADM_Integrated_Logic_DW.PrevY_a - 0.8;
  } else {
    rtb_output_a = rtb_Brake_Torque_Cmd;
  }

  ADM_Integrated_Logic_DW.PrevY_a = rtb_output_a;

  /* End of RateLimiter: '<S3>/TargetSpd_RateLimiter' */

  /* Product: '<S20>/Product1' incorporates:
   *  Constant: '<S20>/Constant'
   *  Gain: '<S20>/gain'
   *  Product: '<S20>/Product11'
   *  Sum: '<S20>/Sum1'
   *  Sum: '<S20>/Sum2'
   *  Sum: '<S20>/Sum3'
   *  Sum: '<S20>/Sum4'
   *  UnitDelay: '<S20>/d'
   *  UnitDelay: '<S20>/d1'
   */
  rtb_Brake_Torque_Cmd = ((rtb_output_a + ADM_Integrated_Logic_DW.d1_DSTATE) *
    0.002 + 0.1041032953945969 * ADM_Integrated_Logic_DW.d_DSTATE) /
    0.1081032953945969;

  /* Sum: '<S30>/Sum1' incorporates:
   *  Gain: '<S30>/gain'
   *  Sum: '<S30>/Sum2'
   *  UnitDelay: '<S30>/d'
   *  UnitDelay: '<S30>/d1'
   */
  rtb_Sum1_i2 = (rtb_Brake_Torque_Cmd - ADM_Integrated_Logic_DW.d_DSTATE_i) *
    1000.0 - ADM_Integrated_Logic_DW.d1_DSTATE_p;

  /* Sum: '<S31>/Sum1' incorporates:
   *  Gain: '<S31>/gain'
   *  Sum: '<S31>/Sum2'
   *  UnitDelay: '<S31>/d'
   *  UnitDelay: '<S31>/d1'
   */
  rtb_Sum1_o1 = (rtb_Sum1_i2 - ADM_Integrated_Logic_DW.d_DSTATE_c) * 1000.0 -
    ADM_Integrated_Logic_DW.d1_DSTATE_h;

  /* Sum: '<S32>/Sum1' incorporates:
   *  Gain: '<S32>/gain'
   *  Sum: '<S32>/Sum2'
   *  UnitDelay: '<S32>/d'
   *  UnitDelay: '<S32>/d1'
   */
  rtb_Sum1_lm = (rtb_Sum1_o1 - ADM_Integrated_Logic_DW.d_DSTATE_d) * 1000.0 -
    ADM_Integrated_Logic_DW.d1_DSTATE_l;

  /* Gain: '<S28>/Gain' incorporates:
   *  Constant: '<S28>/Constant3'
   *  Constant: '<S28>/Constant4'
   *  Constant: '<S28>/Constant5'
   *  Product: '<S28>/Product'
   *  Product: '<S28>/Product1'
   *  Product: '<S28>/Product2'
   *  Sum: '<S28>/Add5'
   */
  rtb_Gain_c = (((156.8 * rtb_Brake_Torque_Cmd + 212.8 * rtb_Sum1_i2) + 21.8 *
                 rtb_Sum1_o1) + rtb_Sum1_lm) * 9.44822373393802E-6;

  /* Product: '<S34>/Product1' incorporates:
   *  Constant: '<S34>/Constant1'
   *  Constant: '<S34>/Constant2'
   *  Delay: '<S34>/Delay'
   *  Delay: '<S34>/Delay1'
   *  Delay: '<S34>/Delay2'
   *  Delay: '<S34>/Delay3'
   *  Gain: '<S34>/gain1'
   *  Gain: '<S34>/gain3'
   *  Product: '<S34>/x(n), x(n-1), x(n-2)'
   *  Product: '<S34>/y(n-1)'
   *  Product: '<S34>/y(n-2)'
   *  Sum: '<S34>/Sum1'
   *  Sum: '<S34>/Sum2'
   *  Sum: '<S34>/Sum3'
   *  Sum: '<S34>/Sum4'
   *  Sum: '<S34>/Sum5'
   *  Sum: '<S34>/Sum6'
   */
  rtb_Product1_pd = ((((2.0 * ADM_Integrated_Logic_DW.Delay1_DSTATE + rtb_Gain_c)
                       + ADM_Integrated_Logic_DW.Delay_DSTATE[0]) *
                      0.39478417604357435 - -7.2104316479128512 *
                      ADM_Integrated_Logic_DW.Delay2_DSTATE) -
                     2.6178993711731877 * ADM_Integrated_Logic_DW.Delay3_DSTATE
                     [0]) / 6.1716689809139611;

  /* Product: '<S33>/Product1' incorporates:
   *  Constant: '<S33>/Constant'
   *  Gain: '<S33>/gain'
   *  Product: '<S33>/Product11'
   *  Sum: '<S33>/Sum1'
   *  Sum: '<S33>/Sum2'
   *  Sum: '<S33>/Sum3'
   *  Sum: '<S33>/Sum4'
   *  UnitDelay: '<S33>/d'
   *  UnitDelay: '<S33>/d1'
   */
  rtb_Product1_m = ((rtb_Product1_pd + ADM_Integrated_Logic_DW.d1_DSTATE_e) *
                    0.002 + 0.00861032953945969 *
                    ADM_Integrated_Logic_DW.d_DSTATE_ij) / 0.01261032953945969;

  /* Sum: '<S3>/Subtract' incorporates:
   *  Inport: '<Root>/GV_MCU_RPM'
   *  MultiPortSwitch: '<S27>/Multiport Switch1'
   */
  rtb_Error_m = rtb_Brake_Torque_Cmd - ADM_Integrated_Logic_U.GV_MCU_RPM;

  /* Saturate: '<S3>/Error_Saturation' incorporates:
   *  MultiPortSwitch: '<S27>/Multiport Switch1'
   */
  if (rtb_Error_m > 2000.0) {
    rtb_Error_m = 2000.0;
  } else if (rtb_Error_m < -2000.0) {
    rtb_Error_m = -2000.0;
  }

  /* End of Saturate: '<S3>/Error_Saturation' */

  /* DeadZone: '<S17>/Dead Zone' incorporates:
   *  MultiPortSwitch: '<S27>/Multiport Switch1'
   */
  if (rtb_Error_m > 50.0) {
    rtb_Add_e = rtb_Error_m - 50.0;
  } else if (rtb_Error_m >= -50.0) {
    rtb_Add_e = 0.0;
  } else {
    rtb_Add_e = rtb_Error_m - -50.0;
  }

  /* Sum: '<S22>/Sum1' incorporates:
   *  Gain: '<S22>/gain'
   *  Sum: '<S22>/Sum2'
   *  UnitDelay: '<S22>/d'
   *  UnitDelay: '<S22>/d1'
   */
  rtb_Error_m = (rtb_Brake_Torque_Cmd - ADM_Integrated_Logic_DW.d_DSTATE_ir) *
    1000.0 - ADM_Integrated_Logic_DW.d1_DSTATE_o;

  /* Sum: '<S23>/Sum1' incorporates:
   *  Gain: '<S23>/gain'
   *  Sum: '<S23>/Sum2'
   *  UnitDelay: '<S23>/d'
   *  UnitDelay: '<S23>/d1'
   */
  rtb_Sum1_aj = (rtb_Error_m - ADM_Integrated_Logic_DW.d_DSTATE_m) * 1000.0 -
    ADM_Integrated_Logic_DW.d1_DSTATE_hm;

  /* Sum: '<S24>/Sum1' incorporates:
   *  Gain: '<S24>/gain'
   *  Sum: '<S24>/Sum2'
   *  UnitDelay: '<S24>/d'
   *  UnitDelay: '<S24>/d1'
   */
  rtb_Sum1_p = (rtb_Sum1_aj - ADM_Integrated_Logic_DW.d_DSTATE_mw) * 1000.0 -
    ADM_Integrated_Logic_DW.d1_DSTATE_g;

  /* Sum: '<S15>/Add3' incorporates:
   *  Constant: '<S15>/Constant3'
   *  Constant: '<S15>/Constant4'
   *  Constant: '<S15>/Constant5'
   *  Gain: '<S15>/Gain'
   *  Memory: '<S3>/Memory'
   *  Product: '<S15>/Product2'
   *  Product: '<S15>/Product3'
   *  Product: '<S15>/Product4'
   *  Sum: '<S15>/Add2'
   */
  rtb_Add3 = (((156.8 * rtb_Brake_Torque_Cmd + 212.8 * rtb_Error_m) + 21.8 *
               rtb_Sum1_aj) + rtb_Sum1_p) * 9.44822373393802E-6 -
    ADM_Integrated_Logic_DW.Memory_PreviousInput;

  /* Product: '<S25>/Product1' incorporates:
   *  Constant: '<S25>/Constant'
   *  Gain: '<S25>/gain'
   *  Product: '<S25>/Product11'
   *  Sum: '<S25>/Sum1'
   *  Sum: '<S25>/Sum2'
   *  Sum: '<S25>/Sum3'
   *  Sum: '<S25>/Sum4'
   *  UnitDelay: '<S25>/d'
   *  UnitDelay: '<S25>/d1'
   */
  rtb_Product1_e = ((rtb_Add3 + ADM_Integrated_Logic_DW.d1_DSTATE_ej) * 0.002 +
                    0.029830988618379066 * ADM_Integrated_Logic_DW.d_DSTATE_j) /
    0.03383098861837907;

  /* Product: '<S26>/Product1' incorporates:
   *  Constant: '<S26>/Constant1'
   *  Constant: '<S26>/Constant2'
   *  Delay: '<S26>/Delay'
   *  Delay: '<S26>/Delay1'
   *  Delay: '<S26>/Delay2'
   *  Delay: '<S26>/Delay3'
   *  Gain: '<S26>/gain1'
   *  Gain: '<S26>/gain3'
   *  Product: '<S26>/x(n), x(n-1), x(n-2)'
   *  Product: '<S26>/y(n-1)'
   *  Product: '<S26>/y(n-2)'
   *  Sum: '<S26>/Sum1'
   *  Sum: '<S26>/Sum2'
   *  Sum: '<S26>/Sum3'
   *  Sum: '<S26>/Sum4'
   *  Sum: '<S26>/Sum5'
   *  Sum: '<S26>/Sum6'
   */
  rtb_Product1_iz = ((((2.0 * ADM_Integrated_Logic_DW.Delay1_DSTATE_c +
                        rtb_Product1_e) +
                       ADM_Integrated_Logic_DW.Delay_DSTATE_p[0]) *
                      0.00035530575843921691 - -7.9992893884831213 *
                      ADM_Integrated_Logic_DW.Delay2_DSTATE_n) -
                     3.9470487616123275 *
                     ADM_Integrated_Logic_DW.Delay3_DSTATE_h[0]) /
    4.0536618499045511;

  /* MATLAB Function: '<S3>/DOB_Gain' incorporates:
   *  Inport: '<Root>/GV_Vx_Fbk'
   */
  /* :  if vx < 5 */
  if (ADM_Integrated_Logic_U.GV_Vx_Fbk < 5.0) {
    /* :  gain = 0; */
    rtb_Switch2 = 0.0;
  } else {
    /* :  else */
    /* :  gain = (vx-5) * 0.2; */
    rtb_Switch2 = (ADM_Integrated_Logic_U.GV_Vx_Fbk - 5.0) * 0.2;
  }

  /* :  if gain >=1 */
  if (rtb_Switch2 >= 1.0) {
    /* :  gain =1; */
    rtb_Switch2 = 1.0;
  }

  /* End of MATLAB Function: '<S3>/DOB_Gain' */

  /* Product: '<S3>/Product1' incorporates:
   *  Constant: '<S3>/DOBFlag'
   *  Product: '<S3>/Product'
   */
  rtb_Integrator_1 = -(rtb_Product1_iz * 0.9);

  /* Saturate: '<S3>/DOB_Saturation' */
  if (rtb_Integrator_1 > 30.0) {
    rtb_Integrator_1 = 30.0;
  } else if (rtb_Integrator_1 < -30.0) {
    rtb_Integrator_1 = -30.0;
  }

  /* Sum: '<S3>/Subtract2' incorporates:
   *  DeadZone: '<S17>/Dead Zone'
   *  MultiPortSwitch: '<S27>/Multiport Switch1'
   *  Product: '<S29>/Product'
   *  Product: '<S3>/Multiply1'
   *  Saturate: '<S3>/DOB_Saturation'
   *  Sum: '<S17>/Add'
   */
  rtb_Add_e = (rtb_Add_e * 0.044648264844923756 + rtb_Product1_m) +
    rtb_Integrator_1 * rtb_Switch2;

  /* Saturate: '<S3>/Torq_Saturation' */
  if (rtb_Add_e > 80.0) {
    rtb_Add_e = 80.0;
  } else if (rtb_Add_e < -80.0) {
    rtb_Add_e = -80.0;
  }

  /* End of Saturate: '<S3>/Torq_Saturation' */

  /* MATLAB Function: '<S1>/Emergency_Motor_Func' */
  /* :  if Emergency_Flag == 1 */
  if (rtb_Compare) {
    /* Outport: '<Root>/GV_Motor_Torque_Cmd' */
    /* :  Emergency_Motor_Output = 0; */
    ADM_Integrated_Logic_Y.GV_Motor_Torque_Cmd = 0.0;
  } else {
    /* Outport: '<Root>/GV_Motor_Torque_Cmd' incorporates:
     *  Product: '<S3>/Multiply'
     */
    /* :  else */
    /* :  Emergency_Motor_Output = Motor_Torque_Cmd; */
    ADM_Integrated_Logic_Y.GV_Motor_Torque_Cmd = rtb_output_p * rtb_Add_e;
  }

  /* End of MATLAB Function: '<S1>/Emergency_Motor_Func' */

  /* Outport: '<Root>/GV_Gear_Postion_Out' */
  ADM_Integrated_Logic_Y.GV_Gear_Postion_Out = rtb_deltafalllimit_m;

  /* Sum: '<S1>/Add2' incorporates:
   *  Inport: '<Root>/GV_Vx_Fbk'
   */
  rtb_Vx_Cmd_R -= ADM_Integrated_Logic_U.GV_Vx_Fbk;

  /* Outport: '<Root>/GV_Hill_Torque_Assist' */
  ADM_Integrated_Logic_Y.GV_Hill_Torque_Assist = rtb_Desired_Torque;

  /* Outport: '<Root>/Debug_HAC_Pitch_angle' */
  ADM_Integrated_Logic_Y.Debug_HAC_Pitch_angle = rtb_Yk1_e;

  /* MATLAB Function: '<S39>/Calculate_F_c1' incorporates:
   *  Gain: '<S39>/Gain3'
   */
  ADM_Integrated_Lo_Calculate_F_c(ADM_Integrated_Logic_ConstB.W_Value_for_Brake,
    0.017453292519943295 * rtb_Yk1_e, &rtb_Switch2);

  /* Product: '<S40>/Product1' incorporates:
   *  Constant: '<S40>/Constant'
   *  Gain: '<S40>/gain'
   *  Inport: '<Root>/GV_IMU_AX_Val'
   *  Product: '<S40>/Product11'
   *  Sum: '<S40>/Sum1'
   *  Sum: '<S40>/Sum2'
   *  Sum: '<S40>/Sum3'
   *  Sum: '<S40>/Sum4'
   *  UnitDelay: '<S40>/d'
   *  UnitDelay: '<S40>/d1'
   */
  rtb_Desired_Torque = ((ADM_Integrated_Logic_U.GV_IMU_AX_Val +
    ADM_Integrated_Logic_DW.d1_DSTATE_ob) * 0.002 + 0.061661977236758134 *
                        ADM_Integrated_Logic_DW.d_DSTATE_e) /
    0.065661977236758137;

  /* Product: '<S41>/Product1' incorporates:
   *  Constant: '<S41>/Constant'
   *  Gain: '<S41>/gain'
   *  Inport: '<Root>/GV_IMU_AY_Val'
   *  Product: '<S41>/Product11'
   *  Sum: '<S41>/Sum1'
   *  Sum: '<S41>/Sum2'
   *  Sum: '<S41>/Sum3'
   *  Sum: '<S41>/Sum4'
   *  UnitDelay: '<S41>/d'
   *  UnitDelay: '<S41>/d1'
   */
  rtb_output_p = ((ADM_Integrated_Logic_U.GV_IMU_AY_Val +
                   ADM_Integrated_Logic_DW.d1_DSTATE_i) * 0.002 +
                  0.061661977236758134 * ADM_Integrated_Logic_DW.d_DSTATE_p) /
    0.065661977236758137;

  /* Product: '<S42>/Product1' incorporates:
   *  Constant: '<S42>/Constant'
   *  Gain: '<S42>/gain'
   *  Inport: '<Root>/GV_IMU_AZ_Val'
   *  Product: '<S42>/Product11'
   *  Sum: '<S42>/Sum1'
   *  Sum: '<S42>/Sum2'
   *  Sum: '<S42>/Sum3'
   *  Sum: '<S42>/Sum4'
   *  UnitDelay: '<S42>/d'
   *  UnitDelay: '<S42>/d1'
   */
  rtb_Integrator_1 = ((ADM_Integrated_Logic_U.GV_IMU_AZ_Val +
                       ADM_Integrated_Logic_DW.d1_DSTATE_o1) * 0.002 +
                      0.061661977236758134 * ADM_Integrated_Logic_DW.d_DSTATE_n)
    / 0.065661977236758137;

  /* MATLAB Function: '<S7>/Pitch_calculate' */
  /*  This function calculates the pitch angle using accelerometer data. */
  /*  Inputs: */
  /*    ax - Acceleration in the x-axis */
  /*    ay - Acceleration in the y-axis */
  /*    az - Acceleration in the z-axis */
  /*  Output: */
  /*    pitch - Calculated pitch angle in degrees */
  /*  Step 1: Calculate the denominator (sqrt(ay^2 + az^2)) */
  /* :  denominator = sqrt(ay^2 + az^2); */
  rtb_Switch2 = sqrt(rtb_output_p * rtb_output_p + rtb_Integrator_1 *
                     rtb_Integrator_1);

  /*  Step 2: Compute pitch in radians using arctan(ax / denominator) */
  /*  Protect against division by zero */
  /* :  if denominator == 0 */
  if (rtb_Switch2 != 0.0) {
    /* :  else */
    /* :  pitch = atan(ax / denominator); */
    rtb_Switch2 = atan(rtb_Desired_Torque / rtb_Switch2);

    /*  Calculate pitch in radians */
  } else {
    /* :  pitch = 0; */
    /*  If ay and az are both zero, pitch is set to 0 */
  }

  /* Sum: '<S7>/Sum' incorporates:
   *  DiscreteIntegrator: '<S7>/Integrator_2'
   *  MATLAB Function: '<S7>/Pitch_calculate'
   */
  /*  Step 3: Convert radians to degrees */
  /* :  pitch = pitch * (180 / pi); */
  /*  Convert to degrees */
  rtb_Sum_f = ADM_Integrated_Logic_DW.Integrator_2_DSTATE - rtb_Switch2 *
    57.295779513082323;

  /* Product: '<S43>/Product1' incorporates:
   *  Constant: '<S43>/Constant'
   *  Gain: '<S43>/gain'
   *  Inport: '<Root>/GV_IMU_PitchRtVal'
   *  Product: '<S43>/Product11'
   *  Sum: '<S43>/Sum1'
   *  Sum: '<S43>/Sum2'
   *  Sum: '<S43>/Sum3'
   *  Sum: '<S43>/Sum4'
   *  UnitDelay: '<S43>/d'
   *  UnitDelay: '<S43>/d1'
   */
  rtb_Product1_f = ((ADM_Integrated_Logic_U.GV_IMU_PitchRtVal +
                     ADM_Integrated_Logic_DW.d1_DSTATE_a) * 0.002 +
                    0.061661977236758134 * ADM_Integrated_Logic_DW.d_DSTATE_d1) /
    0.065661977236758137;

  /* Sum: '<S1>/Add1' incorporates:
   *  Constant: '<S1>/Constant'
   *  Gain: '<S1>/Gain1'
   *  Inport: '<Root>/GV_Vx_Fbk'
   *  Sum: '<S1>/Add3'
   */
  rtb_Add1_tmp = 400.0 - 6.5 * ADM_Integrated_Logic_U.GV_Vx_Fbk;

  /* MATLAB Function: '<S1>/MATLAB Function1' incorporates:
   *  Sum: '<S1>/Add1'
   */
  ADM_Integrated__MATLABFunction1(rtb_Add1_tmp, &rtb_y_l);

  /* Product: '<S12>/delta rise limit' incorporates:
   *  SampleTimeMath: '<S12>/sample time'
   *
   * About '<S12>/sample time':
   *  y = K where K = ( w * Ts )
   *   */
  rtb_Switch2 = rtb_y_l * 0.002;

  /* UnitDelay: '<S12>/Delay Input2'
   *
   * Block description for '<S12>/Delay Input2':
   *
   *  Store in Global RAM
   */
  rtb_Yk1_p = ADM_Integrated_Logic_DW.DelayInput2_DSTATE_c;

  /* Sum: '<S12>/Difference Inputs1' incorporates:
   *  Inport: '<Root>/GV_RWA_RackAngleCommand'
   *
   * Block description for '<S12>/Difference Inputs1':
   *
   *  Add in CPU
   */
  rtb_UkYk1_j = ADM_Integrated_Logic_U.GV_RWA_RackAngleCommand - rtb_Yk1_p;

  /* MATLAB Function: '<S1>/MATLAB Function2' incorporates:
   *  Gain: '<S1>/Gain3'
   *  Sum: '<S1>/Add1'
   */
  ADM_Integrated__MATLABFunction2(-rtb_Add1_tmp, &rtb_y_l);

  /* Switch: '<S53>/Switch2' incorporates:
   *  RelationalOperator: '<S53>/LowerRelop1'
   */
  if (rtb_UkYk1_j <= rtb_Switch2) {
    /* Product: '<S12>/delta fall limit' incorporates:
     *  SampleTimeMath: '<S12>/sample time'
     *
     * About '<S12>/sample time':
     *  y = K where K = ( w * Ts )
     *   */
    rtb_Switch2 = rtb_y_l * 0.002;

    /* Switch: '<S53>/Switch' incorporates:
     *  RelationalOperator: '<S53>/UpperRelop'
     */
    if (rtb_UkYk1_j >= rtb_Switch2) {
      rtb_Switch2 = rtb_UkYk1_j;
    }

    /* End of Switch: '<S53>/Switch' */
  }

  /* End of Switch: '<S53>/Switch2' */

  /* Sum: '<S12>/Difference Inputs2'
   *
   * Block description for '<S12>/Difference Inputs2':
   *
   *  Add in CPU
   */
  rtb_Yk1_p += rtb_Switch2;

  /* Outport: '<Root>/GV_Master_Rack_Angle_Cmd' */
  ADM_Integrated_Logic_Y.GV_Master_Rack_Angle_Cmd = rtb_Yk1_p;

  /* MATLAB Function: '<S1>/MATLAB Function5' */
  ADM_Integrated__MATLABFunction1(rtb_Add1_tmp, &rtb_y_l);

  /* Product: '<S13>/delta rise limit' incorporates:
   *  SampleTimeMath: '<S13>/sample time'
   *
   * About '<S13>/sample time':
   *  y = K where K = ( w * Ts )
   *   */
  rtb_Switch2 = rtb_y_l * 0.002;

  /* UnitDelay: '<S13>/Delay Input2'
   *
   * Block description for '<S13>/Delay Input2':
   *
   *  Store in Global RAM
   */
  rtb_UkYk1_j = ADM_Integrated_Logic_DW.DelayInput2_DSTATE_i;

  /* Sum: '<S13>/Difference Inputs1' incorporates:
   *  Inport: '<Root>/GV_RWS_RackAngleCommand'
   *
   * Block description for '<S13>/Difference Inputs1':
   *
   *  Add in CPU
   */
  rtb_Add1 = ADM_Integrated_Logic_U.GV_RWS_RackAngleCommand - rtb_UkYk1_j;

  /* MATLAB Function: '<S1>/MATLAB Function6' incorporates:
   *  Gain: '<S1>/Gain5'
   */
  ADM_Integrated__MATLABFunction2(-rtb_Add1_tmp, &rtb_y_l);

  /* Switch: '<S54>/Switch2' incorporates:
   *  RelationalOperator: '<S54>/LowerRelop1'
   */
  if (rtb_Add1 <= rtb_Switch2) {
    /* Product: '<S13>/delta fall limit' incorporates:
     *  SampleTimeMath: '<S13>/sample time'
     *
     * About '<S13>/sample time':
     *  y = K where K = ( w * Ts )
     *   */
    rtb_Switch2 = rtb_y_l * 0.002;

    /* Switch: '<S54>/Switch' incorporates:
     *  RelationalOperator: '<S54>/UpperRelop'
     */
    if (rtb_Add1 >= rtb_Switch2) {
      rtb_Switch2 = rtb_Add1;
    }

    /* End of Switch: '<S54>/Switch' */
  }

  /* End of Switch: '<S54>/Switch2' */

  /* Sum: '<S13>/Difference Inputs2'
   *
   * Block description for '<S13>/Difference Inputs2':
   *
   *  Add in CPU
   */
  rtb_Switch2 += rtb_UkYk1_j;

  /* Outport: '<Root>/GV_RWS_RackAngleCmd1' */
  ADM_Integrated_Logic_Y.GV_RWS_RackAngleCmd1 = rtb_Switch2;

  /* Update for UnitDelay: '<S46>/Delay Input2'
   *
   * Block description for '<S46>/Delay Input2':
   *
   *  Store in Global RAM
   */
  ADM_Integrated_Logic_DW.DelayInput2_DSTATE = rtb_UkYk1;

  /* Update for UnitDelay: '<S47>/Delay Input2'
   *
   * Block description for '<S47>/Delay Input2':
   *
   *  Store in Global RAM
   */
  ADM_Integrated_Logic_DW.DelayInput2_DSTATE_m = rtb_Yk1_e;

  /* Update for DiscreteIntegrator: '<S7>/Integrator_2' incorporates:
   *  Constant: '<S7>/I_gain'
   *  Constant: '<S7>/P_gain'
   *  DiscreteIntegrator: '<S7>/Integrator_1'
   *  Product: '<S7>/Product'
   *  Product: '<S7>/Product1'
   *  Sum: '<S7>/Sum1'
   *  Sum: '<S7>/Sum2'
   */
  ADM_Integrated_Logic_DW.Integrator_2_DSTATE += (rtb_Product1_f -
    (ADM_Integrated_Logic_DW.Integrator_1_DSTATE * 5.0 + rtb_Sum_f * 100.0)) *
    0.002;

  /* Update for Delay: '<S6>/Memory' */
  ADM_Integrated_Logic_DW.Memory_DSTATE = rtb_deltafalllimit_m;

  /* Update for DiscreteTransferFcn: '<S1>/Discrete Transfer Fcn' */
  ADM_Integrated_Logic_DW.DiscreteTransferFcn_states = rtb_Vx_Cmd_R - -0.9375 *
    ADM_Integrated_Logic_DW.DiscreteTransferFcn_states;

  /* Update for UnitDelay: '<S20>/d1' */
  ADM_Integrated_Logic_DW.d1_DSTATE = rtb_output_a;

  /* Update for UnitDelay: '<S20>/d' */
  ADM_Integrated_Logic_DW.d_DSTATE = rtb_Brake_Torque_Cmd;

  /* Update for UnitDelay: '<S30>/d' */
  ADM_Integrated_Logic_DW.d_DSTATE_i = rtb_Brake_Torque_Cmd;

  /* Update for UnitDelay: '<S30>/d1' */
  ADM_Integrated_Logic_DW.d1_DSTATE_p = rtb_Sum1_i2;

  /* Update for UnitDelay: '<S31>/d' */
  ADM_Integrated_Logic_DW.d_DSTATE_c = rtb_Sum1_i2;

  /* Update for UnitDelay: '<S31>/d1' */
  ADM_Integrated_Logic_DW.d1_DSTATE_h = rtb_Sum1_o1;

  /* Update for UnitDelay: '<S32>/d' */
  ADM_Integrated_Logic_DW.d_DSTATE_d = rtb_Sum1_o1;

  /* Update for UnitDelay: '<S32>/d1' */
  ADM_Integrated_Logic_DW.d1_DSTATE_l = rtb_Sum1_lm;

  /* Update for Delay: '<S34>/Delay1' */
  ADM_Integrated_Logic_DW.Delay1_DSTATE = rtb_Gain_c;

  /* Update for Delay: '<S34>/Delay' */
  ADM_Integrated_Logic_DW.Delay_DSTATE[0] =
    ADM_Integrated_Logic_DW.Delay_DSTATE[1];
  ADM_Integrated_Logic_DW.Delay_DSTATE[1] = rtb_Gain_c;

  /* Update for Delay: '<S34>/Delay2' */
  ADM_Integrated_Logic_DW.Delay2_DSTATE = rtb_Product1_pd;

  /* Update for Delay: '<S34>/Delay3' */
  ADM_Integrated_Logic_DW.Delay3_DSTATE[0] =
    ADM_Integrated_Logic_DW.Delay3_DSTATE[1];
  ADM_Integrated_Logic_DW.Delay3_DSTATE[1] = rtb_Product1_pd;

  /* Update for UnitDelay: '<S33>/d1' */
  ADM_Integrated_Logic_DW.d1_DSTATE_e = rtb_Product1_pd;

  /* Update for UnitDelay: '<S33>/d' */
  ADM_Integrated_Logic_DW.d_DSTATE_ij = rtb_Product1_m;

  /* Update for Memory: '<S3>/Memory' */
  ADM_Integrated_Logic_DW.Memory_PreviousInput = rtb_Add_e;

  /* Update for UnitDelay: '<S22>/d' */
  ADM_Integrated_Logic_DW.d_DSTATE_ir = rtb_Brake_Torque_Cmd;

  /* Update for UnitDelay: '<S22>/d1' */
  ADM_Integrated_Logic_DW.d1_DSTATE_o = rtb_Error_m;

  /* Update for UnitDelay: '<S23>/d' */
  ADM_Integrated_Logic_DW.d_DSTATE_m = rtb_Error_m;

  /* Update for UnitDelay: '<S23>/d1' */
  ADM_Integrated_Logic_DW.d1_DSTATE_hm = rtb_Sum1_aj;

  /* Update for UnitDelay: '<S24>/d' */
  ADM_Integrated_Logic_DW.d_DSTATE_mw = rtb_Sum1_aj;

  /* Update for UnitDelay: '<S24>/d1' */
  ADM_Integrated_Logic_DW.d1_DSTATE_g = rtb_Sum1_p;

  /* Update for UnitDelay: '<S25>/d1' */
  ADM_Integrated_Logic_DW.d1_DSTATE_ej = rtb_Add3;

  /* Update for UnitDelay: '<S25>/d' */
  ADM_Integrated_Logic_DW.d_DSTATE_j = rtb_Product1_e;

  /* Update for Delay: '<S26>/Delay1' */
  ADM_Integrated_Logic_DW.Delay1_DSTATE_c = rtb_Product1_e;

  /* Update for Delay: '<S26>/Delay' */
  ADM_Integrated_Logic_DW.Delay_DSTATE_p[0] =
    ADM_Integrated_Logic_DW.Delay_DSTATE_p[1];
  ADM_Integrated_Logic_DW.Delay_DSTATE_p[1] = rtb_Product1_e;

  /* Update for Delay: '<S26>/Delay2' */
  ADM_Integrated_Logic_DW.Delay2_DSTATE_n = rtb_Product1_iz;

  /* Update for Delay: '<S26>/Delay3' */
  ADM_Integrated_Logic_DW.Delay3_DSTATE_h[0] =
    ADM_Integrated_Logic_DW.Delay3_DSTATE_h[1];
  ADM_Integrated_Logic_DW.Delay3_DSTATE_h[1] = rtb_Product1_iz;

  /* Update for UnitDelay: '<S40>/d1' incorporates:
   *  Inport: '<Root>/GV_IMU_AX_Val'
   */
  ADM_Integrated_Logic_DW.d1_DSTATE_ob = ADM_Integrated_Logic_U.GV_IMU_AX_Val;

  /* Update for UnitDelay: '<S40>/d' */
  ADM_Integrated_Logic_DW.d_DSTATE_e = rtb_Desired_Torque;

  /* Update for UnitDelay: '<S41>/d1' incorporates:
   *  Inport: '<Root>/GV_IMU_AY_Val'
   */
  ADM_Integrated_Logic_DW.d1_DSTATE_i = ADM_Integrated_Logic_U.GV_IMU_AY_Val;

  /* Update for UnitDelay: '<S41>/d' */
  ADM_Integrated_Logic_DW.d_DSTATE_p = rtb_output_p;

  /* Update for UnitDelay: '<S42>/d1' incorporates:
   *  Inport: '<Root>/GV_IMU_AZ_Val'
   */
  ADM_Integrated_Logic_DW.d1_DSTATE_o1 = ADM_Integrated_Logic_U.GV_IMU_AZ_Val;

  /* Update for UnitDelay: '<S42>/d' */
  ADM_Integrated_Logic_DW.d_DSTATE_n = rtb_Integrator_1;

  /* Update for DiscreteIntegrator: '<S7>/Integrator_1' */
  ADM_Integrated_Logic_DW.Integrator_1_DSTATE += 0.002 * rtb_Sum_f;

  /* Update for UnitDelay: '<S43>/d1' incorporates:
   *  Inport: '<Root>/GV_IMU_PitchRtVal'
   */
  ADM_Integrated_Logic_DW.d1_DSTATE_a = ADM_Integrated_Logic_U.GV_IMU_PitchRtVal;

  /* Update for UnitDelay: '<S43>/d' */
  ADM_Integrated_Logic_DW.d_DSTATE_d1 = rtb_Product1_f;

  /* Update for UnitDelay: '<S12>/Delay Input2'
   *
   * Block description for '<S12>/Delay Input2':
   *
   *  Store in Global RAM
   */
  ADM_Integrated_Logic_DW.DelayInput2_DSTATE_c = rtb_Yk1_p;

  /* Update for UnitDelay: '<S13>/Delay Input2'
   *
   * Block description for '<S13>/Delay Input2':
   *
   *  Store in Global RAM
   */
  ADM_Integrated_Logic_DW.DelayInput2_DSTATE_i = rtb_Switch2;
  if (ADM_Integrated_Logic_M->Timing.TaskCounters.TID[1] == 0) {
    /* Update absolute timer for sample time: [1.0s, 0.0s] */
    /* The "clockTick1" counts the number of times the code of this task has
     * been executed. The resolution of this integer timer is 1.0, which is the step size
     * of the task. Size of "clockTick1" ensures timer will not overflow during the
     * application lifespan selected.
     */
    ADM_Integrated_Logic_M->Timing.clockTick1++;
  }

  rate_scheduler();
}

/* Model initialize function */
void ADM_Integrated_Logic_initialize(void)
{
  /* :  HAC_ON_FLAG = 0; */
  /* :  Smoothed_Torque = 0; */
  /* :  HAC_Desired_Torque = 0; */
  /* :  HAC_ON_Timer = 0; */
}

/* Model terminate function */
void ADM_Integrated_Logic_terminate(void)
{
  /* (no terminate code required) */
}

/*
 * File trailer for generated code.
 *
 * [EOF]
 */