mirror of
https://github.com/Dev-KATECH/ADM.git
synced 2026-05-17 01:43:59 +09:00
1502 lines
44 KiB
C
1502 lines
44 KiB
C
/*
|
||
* 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.63
|
||
* Simulink Coder version : 24.1 (R2024a) 19-Nov-2023
|
||
* C/C++ source code generated on : Wed Jun 4 15:25:59 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: '<S8>/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;
|
||
|
||
/*
|
||
* Output and update for atomic system:
|
||
* '<S40>/Calculate_F_c'
|
||
* '<S40>/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_Sum;
|
||
double rtb_Sum1_aj;
|
||
double rtb_Sum1_i2;
|
||
double rtb_Sum1_lm;
|
||
double rtb_Sum1_o1;
|
||
double rtb_Sum1_p;
|
||
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);
|
||
|
||
/* Product: '<S46>/delta rise limit' incorporates:
|
||
* Constant: '<S8>/Upper_Torq'
|
||
* SampleTimeMath: '<S46>/sample time'
|
||
*
|
||
* About '<S46>/sample time':
|
||
* y = K where K = ( w * Ts )
|
||
* */
|
||
/* : if Emergency_Flag == 1 */
|
||
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: '<S8>/Pitch_Saturation' incorporates:
|
||
* DiscreteIntegrator: '<S8>/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: '<S8>/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: '<S40>/Calculate_F_c' incorporates:
|
||
* Gain: '<S40>/Gain'
|
||
*/
|
||
ADM_Integrated_Lo_Calculate_F_c(ADM_Integrated_Logic_ConstB.W_value,
|
||
0.017453292519943295 * rtb_Yk1_e, &rtb_UkYk1);
|
||
|
||
/* Gain: '<S40>/Gain2' incorporates:
|
||
* Constant: '<S40>/Radius'
|
||
* Product: '<S40>/Multiply3'
|
||
* Sum: '<S40>/Required_Brake_Cal'
|
||
*/
|
||
/* : F_R= C_r*W*cos(theta); */
|
||
rtb_Integrator_1 = rtb_UkYk1 * 0.292 * 0.083822296730930432;
|
||
|
||
/* Saturate: '<S40>/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: '<S40>/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: '<S8>/Grade_GAIN' */
|
||
rtb_Desired_Torque = 0.8 * rtb_UkYk1;
|
||
|
||
/* MATLAB Function: '<S15>/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: '<S15>/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: '<S36>/Compare' incorporates:
|
||
* Constant: '<S36>/Constant'
|
||
* Inport: '<Root>/GV_BrakeTorqueCommand'
|
||
*/
|
||
rtb_Compare_d = (ADM_Integrated_Logic_U.GV_BrakeTorqueCommand >= 100.0);
|
||
|
||
/* Delay: '<S7>/Memory' */
|
||
rtb_deltafalllimit_m = ADM_Integrated_Logic_DW.Memory_DSTATE;
|
||
|
||
/* MATLAB Function: '<S7>/Gear_FUNCTION1' incorporates:
|
||
* Inport: '<Root>/GV_VCU_GearSelStat'
|
||
* Inport: '<Root>/GV_Vx_Fbk'
|
||
*/
|
||
/* : SAFE_SPEED_THRESHOLD = 0; */
|
||
/* <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> <20>ӵ<EFBFBD>(kph) */
|
||
/* <20>⺻<EFBFBD><E2BABB>: <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> */
|
||
/* : output = Current_Gear; */
|
||
/* 1. P<><50> <20><>û<EFBFBD><C3BB> <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>: <20><><EFBFBD><EFBFBD> <20><><EFBFBD>¿<EFBFBD><C2BF><EFBFBD> <20>켱 ó<><C3B3> */
|
||
/* : 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<><50> <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> */
|
||
} else {
|
||
/* : else */
|
||
/* : output = Current_Gear; */
|
||
/* <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> <20><><EFBFBD><EFBFBD> */
|
||
}
|
||
} else {
|
||
/* 2. <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> <20><><EFBFBD>¿<EFBFBD> <20><><EFBFBD><EFBFBD> ó<><C3B3> */
|
||
/* : switch Current_Gear */
|
||
switch ((int32_t)rtb_deltafalllimit_m) {
|
||
case 0:
|
||
/* : case 0 */
|
||
/* P <20><><EFBFBD><EFBFBD> */
|
||
/* : 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 (<28><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> N<><4E> <20><><EFBFBD>ľ<EFBFBD> <20>ϹǷ<CFB9>) */
|
||
}
|
||
break;
|
||
|
||
case 1:
|
||
/* : case 1 */
|
||
/* R <20><><EFBFBD><EFBFBD> */
|
||
/* : if Gear_Cmd == 2 */
|
||
if (ADM_Integrated_Logic_U.GV_VCU_GearSelStat == 2.0) {
|
||
/* : output = 2; */
|
||
rtb_deltafalllimit_m = 2.0;
|
||
|
||
/* R -> N <20><><EFBFBD><EFBFBD> */
|
||
}
|
||
break;
|
||
|
||
case 2:
|
||
/* : case 2 */
|
||
/* N <20><><EFBFBD><EFBFBD> */
|
||
/* : 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 <20><><EFBFBD><EFBFBD> */
|
||
/* : if Gear_Cmd == 2 */
|
||
if (ADM_Integrated_Logic_U.GV_VCU_GearSelStat == 2.0) {
|
||
/* : output = 2; */
|
||
rtb_deltafalllimit_m = 2.0;
|
||
|
||
/* D -> N <20><><EFBFBD><EFBFBD> */
|
||
}
|
||
break;
|
||
}
|
||
}
|
||
|
||
/* End of MATLAB Function: '<S7>/Gear_FUNCTION1' */
|
||
|
||
/* Chart: '<S8>/Chart' incorporates:
|
||
* Constant: '<S8>/Constant'
|
||
* Constant: '<S8>/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: '<S8>/Chart' */
|
||
|
||
/* MATLAB Function: '<S8>/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: '<S8>/HAC_OFF_OK_Func'
|
||
*/
|
||
/* : HAC_ON_OFF_CHECK = 0; */
|
||
ADM_Integrated_Logic_Y.Debug_HAC_FLAG = 0.0;
|
||
|
||
/* MATLAB Function: '<S8>/HAC_OFF_OK_Func' incorporates:
|
||
* Constant: '<S8>/Accel_Cmd '
|
||
* Constant: '<S8>/Brake_Cmd'
|
||
* Constant: '<S8>/Gear_D'
|
||
* Inport: '<Root>/GV_BrakeTorqueCommand'
|
||
* Inport: '<Root>/GV_MCU_EstTrq'
|
||
* Outport: '<Root>/Debug_HAC_RPM_Decision'
|
||
* RelationalOperator: '<S8>/Relational Operator'
|
||
* RelationalOperator: '<S8>/Relational Operator1'
|
||
* RelationalOperator: '<S8>/Relational Operator2'
|
||
* Sum: '<S8>/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 = 1000.0;
|
||
|
||
/* : Smoothed_Torque = Desired_Brake_Torque_Cmd; */
|
||
ADM_Integrated_Logic_DW.Smoothed_Torque = 1000.0;
|
||
|
||
/* : 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 = 1000.0;
|
||
|
||
/* : Smoothed_Torque = Desired_Brake_Torque_Cmd; */
|
||
ADM_Integrated_Logic_DW.Smoothed_Torque = 1000.0;
|
||
|
||
/* 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 <20>ܿ<EFBFBD><DCBF><EFBFBD> - target rpm <20><> */
|
||
/* : 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 <20>ܿ<EFBFBD><DCBF><EFBFBD> - target rpm <20><> */
|
||
/* : 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: '<S21>/Product1' incorporates:
|
||
* Constant: '<S21>/Constant'
|
||
* Gain: '<S21>/gain'
|
||
* Product: '<S21>/Product11'
|
||
* Sum: '<S21>/Sum1'
|
||
* Sum: '<S21>/Sum2'
|
||
* Sum: '<S21>/Sum3'
|
||
* Sum: '<S21>/Sum4'
|
||
* UnitDelay: '<S21>/d'
|
||
* UnitDelay: '<S21>/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: '<S31>/Sum1' incorporates:
|
||
* Gain: '<S31>/gain'
|
||
* Sum: '<S31>/Sum2'
|
||
* UnitDelay: '<S31>/d'
|
||
* UnitDelay: '<S31>/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: '<S32>/Sum1' incorporates:
|
||
* Gain: '<S32>/gain'
|
||
* Sum: '<S32>/Sum2'
|
||
* UnitDelay: '<S32>/d'
|
||
* UnitDelay: '<S32>/d1'
|
||
*/
|
||
rtb_Sum1_o1 = (rtb_Sum1_i2 - ADM_Integrated_Logic_DW.d_DSTATE_c) * 1000.0 -
|
||
ADM_Integrated_Logic_DW.d1_DSTATE_h;
|
||
|
||
/* Sum: '<S33>/Sum1' incorporates:
|
||
* Gain: '<S33>/gain'
|
||
* Sum: '<S33>/Sum2'
|
||
* UnitDelay: '<S33>/d'
|
||
* UnitDelay: '<S33>/d1'
|
||
*/
|
||
rtb_Sum1_lm = (rtb_Sum1_o1 - ADM_Integrated_Logic_DW.d_DSTATE_d) * 1000.0 -
|
||
ADM_Integrated_Logic_DW.d1_DSTATE_l;
|
||
|
||
/* Gain: '<S29>/Gain' incorporates:
|
||
* Constant: '<S29>/Constant3'
|
||
* Constant: '<S29>/Constant4'
|
||
* Constant: '<S29>/Constant5'
|
||
* Product: '<S29>/Product'
|
||
* Product: '<S29>/Product1'
|
||
* Product: '<S29>/Product2'
|
||
* Sum: '<S29>/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: '<S35>/Product1' incorporates:
|
||
* Constant: '<S35>/Constant1'
|
||
* Constant: '<S35>/Constant2'
|
||
* Delay: '<S35>/Delay'
|
||
* Delay: '<S35>/Delay1'
|
||
* Delay: '<S35>/Delay2'
|
||
* Delay: '<S35>/Delay3'
|
||
* Gain: '<S35>/gain1'
|
||
* Gain: '<S35>/gain3'
|
||
* Product: '<S35>/x(n), x(n-1), x(n-2)'
|
||
* Product: '<S35>/y(n-1)'
|
||
* Product: '<S35>/y(n-2)'
|
||
* Sum: '<S35>/Sum1'
|
||
* Sum: '<S35>/Sum2'
|
||
* Sum: '<S35>/Sum3'
|
||
* Sum: '<S35>/Sum4'
|
||
* Sum: '<S35>/Sum5'
|
||
* Sum: '<S35>/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: '<S34>/Product1' incorporates:
|
||
* Constant: '<S34>/Constant'
|
||
* Gain: '<S34>/gain'
|
||
* Product: '<S34>/Product11'
|
||
* Sum: '<S34>/Sum1'
|
||
* Sum: '<S34>/Sum2'
|
||
* Sum: '<S34>/Sum3'
|
||
* Sum: '<S34>/Sum4'
|
||
* UnitDelay: '<S34>/d'
|
||
* UnitDelay: '<S34>/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: '<S28>/Multiport Switch1'
|
||
*/
|
||
rtb_Error_m = rtb_Brake_Torque_Cmd - ADM_Integrated_Logic_U.GV_MCU_RPM;
|
||
|
||
/* Saturate: '<S3>/Error_Saturation' incorporates:
|
||
* MultiPortSwitch: '<S28>/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: '<S18>/Dead Zone' incorporates:
|
||
* MultiPortSwitch: '<S28>/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: '<S23>/Sum1' incorporates:
|
||
* Gain: '<S23>/gain'
|
||
* Sum: '<S23>/Sum2'
|
||
* UnitDelay: '<S23>/d'
|
||
* UnitDelay: '<S23>/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: '<S24>/Sum1' incorporates:
|
||
* Gain: '<S24>/gain'
|
||
* Sum: '<S24>/Sum2'
|
||
* UnitDelay: '<S24>/d'
|
||
* UnitDelay: '<S24>/d1'
|
||
*/
|
||
rtb_Sum1_aj = (rtb_Error_m - ADM_Integrated_Logic_DW.d_DSTATE_m) * 1000.0 -
|
||
ADM_Integrated_Logic_DW.d1_DSTATE_hm;
|
||
|
||
/* Sum: '<S25>/Sum1' incorporates:
|
||
* Gain: '<S25>/gain'
|
||
* Sum: '<S25>/Sum2'
|
||
* UnitDelay: '<S25>/d'
|
||
* UnitDelay: '<S25>/d1'
|
||
*/
|
||
rtb_Sum1_p = (rtb_Sum1_aj - ADM_Integrated_Logic_DW.d_DSTATE_mw) * 1000.0 -
|
||
ADM_Integrated_Logic_DW.d1_DSTATE_g;
|
||
|
||
/* Sum: '<S16>/Add3' incorporates:
|
||
* Constant: '<S16>/Constant3'
|
||
* Constant: '<S16>/Constant4'
|
||
* Constant: '<S16>/Constant5'
|
||
* Gain: '<S16>/Gain'
|
||
* Memory: '<S3>/Memory'
|
||
* Product: '<S16>/Product2'
|
||
* Product: '<S16>/Product3'
|
||
* Product: '<S16>/Product4'
|
||
* Sum: '<S16>/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: '<S26>/Product1' incorporates:
|
||
* Constant: '<S26>/Constant'
|
||
* Gain: '<S26>/gain'
|
||
* Product: '<S26>/Product11'
|
||
* Sum: '<S26>/Sum1'
|
||
* Sum: '<S26>/Sum2'
|
||
* Sum: '<S26>/Sum3'
|
||
* Sum: '<S26>/Sum4'
|
||
* UnitDelay: '<S26>/d'
|
||
* UnitDelay: '<S26>/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: '<S27>/Product1' incorporates:
|
||
* Constant: '<S27>/Constant1'
|
||
* Constant: '<S27>/Constant2'
|
||
* Delay: '<S27>/Delay'
|
||
* Delay: '<S27>/Delay1'
|
||
* Delay: '<S27>/Delay2'
|
||
* Delay: '<S27>/Delay3'
|
||
* Gain: '<S27>/gain1'
|
||
* Gain: '<S27>/gain3'
|
||
* Product: '<S27>/x(n), x(n-1), x(n-2)'
|
||
* Product: '<S27>/y(n-1)'
|
||
* Product: '<S27>/y(n-2)'
|
||
* Sum: '<S27>/Sum1'
|
||
* Sum: '<S27>/Sum2'
|
||
* Sum: '<S27>/Sum3'
|
||
* Sum: '<S27>/Sum4'
|
||
* Sum: '<S27>/Sum5'
|
||
* Sum: '<S27>/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: '<S18>/Dead Zone'
|
||
* MultiPortSwitch: '<S28>/Multiport Switch1'
|
||
* Product: '<S30>/Product'
|
||
* Product: '<S3>/Multiply1'
|
||
* Saturate: '<S3>/DOB_Saturation'
|
||
* Sum: '<S18>/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: '<S40>/Calculate_F_c1' incorporates:
|
||
* Gain: '<S40>/Gain3'
|
||
*/
|
||
ADM_Integrated_Lo_Calculate_F_c(ADM_Integrated_Logic_ConstB.W_Value_for_Brake,
|
||
0.017453292519943295 * rtb_Yk1_e, &rtb_Switch2);
|
||
|
||
/* Product: '<S41>/Product1' incorporates:
|
||
* Constant: '<S41>/Constant'
|
||
* Gain: '<S41>/gain'
|
||
* Inport: '<Root>/GV_IMU_AX_Val'
|
||
* Product: '<S41>/Product11'
|
||
* Sum: '<S41>/Sum1'
|
||
* Sum: '<S41>/Sum2'
|
||
* Sum: '<S41>/Sum3'
|
||
* Sum: '<S41>/Sum4'
|
||
* UnitDelay: '<S41>/d'
|
||
* UnitDelay: '<S41>/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: '<S42>/Product1' incorporates:
|
||
* Constant: '<S42>/Constant'
|
||
* Gain: '<S42>/gain'
|
||
* Inport: '<Root>/GV_IMU_AY_Val'
|
||
* Product: '<S42>/Product11'
|
||
* Sum: '<S42>/Sum1'
|
||
* Sum: '<S42>/Sum2'
|
||
* Sum: '<S42>/Sum3'
|
||
* Sum: '<S42>/Sum4'
|
||
* UnitDelay: '<S42>/d'
|
||
* UnitDelay: '<S42>/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: '<S43>/Product1' incorporates:
|
||
* Constant: '<S43>/Constant'
|
||
* Gain: '<S43>/gain'
|
||
* Inport: '<Root>/GV_IMU_AZ_Val'
|
||
* Product: '<S43>/Product11'
|
||
* Sum: '<S43>/Sum1'
|
||
* Sum: '<S43>/Sum2'
|
||
* Sum: '<S43>/Sum3'
|
||
* Sum: '<S43>/Sum4'
|
||
* UnitDelay: '<S43>/d'
|
||
* UnitDelay: '<S43>/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: '<S8>/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: '<S8>/Sum' incorporates:
|
||
* DiscreteIntegrator: '<S8>/Integrator_2'
|
||
* MATLAB Function: '<S8>/Pitch_calculate'
|
||
*/
|
||
/* Step 3: Convert radians to degrees */
|
||
/* : pitch = pitch * (180 / pi); */
|
||
/* Convert to degrees */
|
||
rtb_Sum = ADM_Integrated_Logic_DW.Integrator_2_DSTATE - rtb_Switch2 *
|
||
57.295779513082323;
|
||
|
||
/* Product: '<S44>/Product1' incorporates:
|
||
* Constant: '<S44>/Constant'
|
||
* Gain: '<S44>/gain'
|
||
* Inport: '<Root>/GV_IMU_PitchRtVal'
|
||
* Product: '<S44>/Product11'
|
||
* Sum: '<S44>/Sum1'
|
||
* Sum: '<S44>/Sum2'
|
||
* Sum: '<S44>/Sum3'
|
||
* Sum: '<S44>/Sum4'
|
||
* UnitDelay: '<S44>/d'
|
||
* UnitDelay: '<S44>/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: '<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_Yk1_p = ADM_Integrated_Logic_DW.DelayInput2_DSTATE_c;
|
||
|
||
/* Sum: '<S13>/Difference Inputs1' incorporates:
|
||
* Inport: '<Root>/GV_RWA_RackAngleCommand'
|
||
*
|
||
* Block description for '<S13>/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: '<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: '<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: '<S13>/Difference Inputs2'
|
||
*
|
||
* Block description for '<S13>/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: '<S14>/delta rise limit' incorporates:
|
||
* SampleTimeMath: '<S14>/sample time'
|
||
*
|
||
* About '<S14>/sample time':
|
||
* y = K where K = ( w * Ts )
|
||
* */
|
||
rtb_Switch2 = rtb_y_l * 0.002;
|
||
|
||
/* UnitDelay: '<S14>/Delay Input2'
|
||
*
|
||
* Block description for '<S14>/Delay Input2':
|
||
*
|
||
* Store in Global RAM
|
||
*/
|
||
rtb_UkYk1_j = ADM_Integrated_Logic_DW.DelayInput2_DSTATE_i;
|
||
|
||
/* Sum: '<S14>/Difference Inputs1' incorporates:
|
||
* Inport: '<Root>/GV_RWS_RackAngleCommand'
|
||
*
|
||
* Block description for '<S14>/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: '<S14>/delta fall limit' incorporates:
|
||
* SampleTimeMath: '<S14>/sample time'
|
||
*
|
||
* About '<S14>/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: '<S14>/Difference Inputs2'
|
||
*
|
||
* Block description for '<S14>/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: '<S8>/Integrator_2' incorporates:
|
||
* Constant: '<S8>/I_gain'
|
||
* Constant: '<S8>/P_gain'
|
||
* DiscreteIntegrator: '<S8>/Integrator_1'
|
||
* Product: '<S8>/Product'
|
||
* Product: '<S8>/Product1'
|
||
* Sum: '<S8>/Sum1'
|
||
* Sum: '<S8>/Sum2'
|
||
*/
|
||
ADM_Integrated_Logic_DW.Integrator_2_DSTATE += (rtb_Product1_f -
|
||
(ADM_Integrated_Logic_DW.Integrator_1_DSTATE * 5.0 + rtb_Sum * 100.0)) *
|
||
0.002;
|
||
|
||
/* Update for Delay: '<S7>/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: '<S21>/d1' */
|
||
ADM_Integrated_Logic_DW.d1_DSTATE = rtb_output_a;
|
||
|
||
/* Update for UnitDelay: '<S21>/d' */
|
||
ADM_Integrated_Logic_DW.d_DSTATE = rtb_Brake_Torque_Cmd;
|
||
|
||
/* Update for UnitDelay: '<S31>/d' */
|
||
ADM_Integrated_Logic_DW.d_DSTATE_i = rtb_Brake_Torque_Cmd;
|
||
|
||
/* Update for UnitDelay: '<S31>/d1' */
|
||
ADM_Integrated_Logic_DW.d1_DSTATE_p = rtb_Sum1_i2;
|
||
|
||
/* Update for UnitDelay: '<S32>/d' */
|
||
ADM_Integrated_Logic_DW.d_DSTATE_c = rtb_Sum1_i2;
|
||
|
||
/* Update for UnitDelay: '<S32>/d1' */
|
||
ADM_Integrated_Logic_DW.d1_DSTATE_h = rtb_Sum1_o1;
|
||
|
||
/* Update for UnitDelay: '<S33>/d' */
|
||
ADM_Integrated_Logic_DW.d_DSTATE_d = rtb_Sum1_o1;
|
||
|
||
/* Update for UnitDelay: '<S33>/d1' */
|
||
ADM_Integrated_Logic_DW.d1_DSTATE_l = rtb_Sum1_lm;
|
||
|
||
/* Update for Delay: '<S35>/Delay1' */
|
||
ADM_Integrated_Logic_DW.Delay1_DSTATE = rtb_Gain_c;
|
||
|
||
/* Update for Delay: '<S35>/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: '<S35>/Delay2' */
|
||
ADM_Integrated_Logic_DW.Delay2_DSTATE = rtb_Product1_pd;
|
||
|
||
/* Update for Delay: '<S35>/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: '<S34>/d1' */
|
||
ADM_Integrated_Logic_DW.d1_DSTATE_e = rtb_Product1_pd;
|
||
|
||
/* Update for UnitDelay: '<S34>/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: '<S23>/d' */
|
||
ADM_Integrated_Logic_DW.d_DSTATE_ir = rtb_Brake_Torque_Cmd;
|
||
|
||
/* Update for UnitDelay: '<S23>/d1' */
|
||
ADM_Integrated_Logic_DW.d1_DSTATE_o = rtb_Error_m;
|
||
|
||
/* Update for UnitDelay: '<S24>/d' */
|
||
ADM_Integrated_Logic_DW.d_DSTATE_m = rtb_Error_m;
|
||
|
||
/* Update for UnitDelay: '<S24>/d1' */
|
||
ADM_Integrated_Logic_DW.d1_DSTATE_hm = rtb_Sum1_aj;
|
||
|
||
/* Update for UnitDelay: '<S25>/d' */
|
||
ADM_Integrated_Logic_DW.d_DSTATE_mw = rtb_Sum1_aj;
|
||
|
||
/* Update for UnitDelay: '<S25>/d1' */
|
||
ADM_Integrated_Logic_DW.d1_DSTATE_g = rtb_Sum1_p;
|
||
|
||
/* Update for UnitDelay: '<S26>/d1' */
|
||
ADM_Integrated_Logic_DW.d1_DSTATE_ej = rtb_Add3;
|
||
|
||
/* Update for UnitDelay: '<S26>/d' */
|
||
ADM_Integrated_Logic_DW.d_DSTATE_j = rtb_Product1_e;
|
||
|
||
/* Update for Delay: '<S27>/Delay1' */
|
||
ADM_Integrated_Logic_DW.Delay1_DSTATE_c = rtb_Product1_e;
|
||
|
||
/* Update for Delay: '<S27>/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: '<S27>/Delay2' */
|
||
ADM_Integrated_Logic_DW.Delay2_DSTATE_n = rtb_Product1_iz;
|
||
|
||
/* Update for Delay: '<S27>/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: '<S41>/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: '<S41>/d' */
|
||
ADM_Integrated_Logic_DW.d_DSTATE_e = rtb_Desired_Torque;
|
||
|
||
/* Update for UnitDelay: '<S42>/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: '<S42>/d' */
|
||
ADM_Integrated_Logic_DW.d_DSTATE_p = rtb_output_p;
|
||
|
||
/* Update for UnitDelay: '<S43>/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: '<S43>/d' */
|
||
ADM_Integrated_Logic_DW.d_DSTATE_n = rtb_Integrator_1;
|
||
|
||
/* Update for DiscreteIntegrator: '<S8>/Integrator_1' */
|
||
ADM_Integrated_Logic_DW.Integrator_1_DSTATE += 0.002 * rtb_Sum;
|
||
|
||
/* Update for UnitDelay: '<S44>/d1' incorporates:
|
||
* Inport: '<Root>/GV_IMU_PitchRtVal'
|
||
*/
|
||
ADM_Integrated_Logic_DW.d1_DSTATE_a = ADM_Integrated_Logic_U.GV_IMU_PitchRtVal;
|
||
|
||
/* Update for UnitDelay: '<S44>/d' */
|
||
ADM_Integrated_Logic_DW.d_DSTATE_d1 = rtb_Product1_f;
|
||
|
||
/* Update for UnitDelay: '<S13>/Delay Input2'
|
||
*
|
||
* Block description for '<S13>/Delay Input2':
|
||
*
|
||
* Store in Global RAM
|
||
*/
|
||
ADM_Integrated_Logic_DW.DelayInput2_DSTATE_c = rtb_Yk1_p;
|
||
|
||
/* Update for UnitDelay: '<S14>/Delay Input2'
|
||
*
|
||
* Block description for '<S14>/Delay Input2':
|
||
*
|
||
* Store in Global RAM
|
||
*/
|
||
ADM_Integrated_Logic_DW.DelayInput2_DSTATE_i = rtb_Switch2;
|
||
}
|
||
|
||
/* 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]
|
||
*/
|