/* * 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 : 14.2 * Simulink Coder version : 24.2 (R2024b) 21-Jun-2024 * C/C++ source code generated on : Tue Mar 10 17:51:08 2026 * * 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 #include #include /* Named constants for Chart: '/Chart' */ #define IN_HAC_OFF ((uint8_t)1U) #define 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 ADM_Integrated_Lo_Calculate_F_c(double rtu_W, double rtu_theta, double *rty_F_c); static void ADM_Integrated__MATLABFunction1(double rtu_u, double *rty_y); static void ADM_Integrated__MATLABFunction2(double rtu_u, double *rty_y); /* * Output and update for atomic system: * '/Calculate_F_c' * '/Calculate_F_c1' */ static void ADM_Integrated_Lo_Calculate_F_c(double rtu_W, double rtu_theta, double *rty_F_c) { *rty_F_c = rtu_W * sin(rtu_theta); } /* * Output and update for atomic system: * '/MATLAB Function1' * '/MATLAB Function5' */ static void ADM_Integrated__MATLABFunction1(double rtu_u, double *rty_y) { *rty_y = rtu_u; if (rtu_u < 140.0) { *rty_y = 140.0; } } /* * Output and update for atomic system: * '/MATLAB Function2' * '/MATLAB Function6' */ static void ADM_Integrated__MATLABFunction2(double rtu_u, double *rty_y) { *rty_y = rtu_u; if (rtu_u > -140.0) { *rty_y = -140.0; } } /* Model step function */ void ADM_Integrated_Logic_step(void) { double tmp[9]; double x_hat_pred[3]; double tmp_0[2]; double tmp_1[2]; double rtb_AccFlag; double rtb_Add1; double rtb_Add3; double rtb_Add_e; double rtb_Brake_Saturation; double rtb_Brake_Torque_Cmd; double rtb_Desired_Torque; double rtb_Error_m; double rtb_F_PI; double rtb_Gain_c; double rtb_Integrator_1; double rtb_Memory; double rtb_Pitch_Angle; double rtb_Product1; double rtb_Product1_gg; double rtb_Product1_h; double rtb_Product1_k3; double rtb_Product1_m; double rtb_Product1_pd; double rtb_Product2_a; double rtb_Saturation; double rtb_Sum1_cr; double rtb_Sum1_d; double rtb_Sum1_f; double rtb_Sum1_j1; double rtb_Sum_f; double rtb_Switch2_k; double rtb_TSamp; double rtb_Trig; double rtb_Vx_Cmd_R; double rtb_y; double rtb_y_h; int32_t i; bool rtb_Compare_l; bool rtb_Compare_p; static const int8_t a[6] = { 1, 0, 0, 1, 0, 0 }; static const double b_a[6] = { 0.2997, 0.0, -0.0842, 0.01, 0.9981, 0.0 }; double rtb_Add1_tmp; /* Product: '/Product1' incorporates: * Constant: '/Constant' * Gain: '/gain' * Inport: '/GV_IMU_AX_Val' * Product: '/Product11' * Sum: '/Sum1' * Sum: '/Sum2' * Sum: '/Sum3' * Sum: '/Sum4' * UnitDelay: '/d' * UnitDelay: '/d1' */ rtb_Product1 = ((ADM_Integrated_Logic_U.GV_IMU_AX_Val + ADM_Integrated_Logic_DW.d1_DSTATE) * 0.002 + 0.63461977236758138 * ADM_Integrated_Logic_DW.d_DSTATE) / 0.63861977236758138; /* MATLAB Function: '/MATLAB Function' incorporates: * Constant: '/Constant1' * Constant: '/Constant2' */ tmp[0] = 1.0; tmp[3] = 0.002; tmp[6] = -0.019620000000000002; tmp[1] = 0.0; tmp[2] = 0.0; tmp[4] = 1.0; tmp[5] = 0.0; tmp[7] = 0.0; tmp[8] = 1.0; rtb_Pitch_Angle = ADM_Integrated_Logic_DW.x_hat[1]; rtb_Brake_Saturation = ADM_Integrated_Logic_DW.x_hat[0]; rtb_Vx_Cmd_R = ADM_Integrated_Logic_DW.x_hat[2]; for (i = 0; i < 3; i++) { x_hat_pred[i] = (tmp[i + 3] * rtb_Pitch_Angle + tmp[i] * rtb_Brake_Saturation) + tmp[i + 6] * rtb_Vx_Cmd_R; } /* SignalConversion generated from: '/ SFunction ' incorporates: * Inport: '/GV_Vx_Fbk' * MATLAB Function: '/MATLAB Function' */ tmp_0[0] = ADM_Integrated_Logic_U.GV_Vx_Fbk; tmp_0[1] = rtb_Product1; /* MATLAB Function: '/MATLAB Function' */ rtb_Pitch_Angle = x_hat_pred[1]; rtb_Brake_Saturation = x_hat_pred[0]; rtb_Vx_Cmd_R = x_hat_pred[2]; for (i = 0; i < 2; i++) { tmp_1[i] = tmp_0[i] - (((double)a[i + 2] * rtb_Pitch_Angle + (double)a[i] * rtb_Brake_Saturation) + 0.0 * rtb_Vx_Cmd_R); } rtb_Add_e = tmp_1[1]; rtb_Pitch_Angle = tmp_1[0]; for (i = 0; i < 3; i++) { ADM_Integrated_Logic_DW.x_hat[i] = (b_a[i + 3] * rtb_Add_e + b_a[i] * rtb_Pitch_Angle) + x_hat_pred[i]; } /* RelationalOperator: '/Compare' incorporates: * Constant: '/Constant' * Inport: '/GV_Drive_Mode' */ rtb_Compare_l = (ADM_Integrated_Logic_U.GV_Drive_Mode == 2.0); /* MATLAB Function: '/Vx_OutPut_Function' incorporates: * Constant: '/Constant' * Inport: '/GV_BrakeTorqueCommand' * Inport: '/GV_Vx_Command' * Inport: '/GV_Vx_Limit' * RelationalOperator: '/Compare' */ if (!(ADM_Integrated_Logic_U.GV_BrakeTorqueCommand >= 50.0)) { if (ADM_Integrated_Logic_U.GV_Vx_Limit <= ADM_Integrated_Logic_U.GV_Vx_Command) { rtb_Add_e = ADM_Integrated_Logic_U.GV_Vx_Limit; } else { rtb_Add_e = ADM_Integrated_Logic_U.GV_Vx_Command; } } else { rtb_Add_e = 0.0; } /* Product: '/Product' incorporates: * MATLAB Function: '/Vx_OutPut_Function' * Switch: '/Switch' */ rtb_Pitch_Angle = rtb_Add_e * (double)!rtb_Compare_l; /* RateLimiter: '/Input_Vx_RateLimiter' */ rtb_Brake_Saturation = rtb_Pitch_Angle - ADM_Integrated_Logic_DW.PrevY; if (rtb_Brake_Saturation > 0.004) { rtb_Vx_Cmd_R = ADM_Integrated_Logic_DW.PrevY + 0.004; } else if (rtb_Brake_Saturation < -0.008) { rtb_Vx_Cmd_R = ADM_Integrated_Logic_DW.PrevY - 0.008; } else { rtb_Vx_Cmd_R = rtb_Pitch_Angle; } ADM_Integrated_Logic_DW.PrevY = rtb_Vx_Cmd_R; /* End of RateLimiter: '/Input_Vx_RateLimiter' */ /* Saturate: '/Pitch_Saturation' incorporates: * DiscreteIntegrator: '/Integrator_2' */ if (ADM_Integrated_Logic_DW.Integrator_2_DSTATE > 8.0) { rtb_Pitch_Angle = 8.0; } else if (ADM_Integrated_Logic_DW.Integrator_2_DSTATE < -8.0) { rtb_Pitch_Angle = -8.0; } else { rtb_Pitch_Angle = ADM_Integrated_Logic_DW.Integrator_2_DSTATE; } /* End of Saturate: '/Pitch_Saturation' */ /* Delay: '/Delay' */ if (ADM_Integrated_Logic_DW.icLoad) { ADM_Integrated_Logic_DW.Delay_DSTATE = rtb_Pitch_Angle; } /* Product: '/delta rise limit' incorporates: * Constant: '/Angle_Upper' * SampleTimeMath: '/sample time' * * About '/sample time': * y = K where K = ( w * Ts ) * */ rtb_Product2_a = 0.002; /* Sum: '/Difference Inputs1' incorporates: * Delay: '/Delay' * * Block description for '/Difference Inputs1': * * Add in CPU */ rtb_Pitch_Angle -= ADM_Integrated_Logic_DW.Delay_DSTATE; /* Switch: '/Switch2' incorporates: * RelationalOperator: '/LowerRelop1' */ if (!(rtb_Pitch_Angle > 0.002)) { /* Switch: '/Switch' incorporates: * RelationalOperator: '/UpperRelop' */ if (rtb_Pitch_Angle < -0.002) { rtb_Product2_a = -0.002; } else { rtb_Product2_a = rtb_Pitch_Angle; } /* End of Switch: '/Switch' */ } /* End of Switch: '/Switch2' */ /* Sum: '/Difference Inputs2' incorporates: * Delay: '/Delay' * * Block description for '/Difference Inputs2': * * Add in CPU */ rtb_Pitch_Angle = rtb_Product2_a + ADM_Integrated_Logic_DW.Delay_DSTATE; /* MATLAB Function: '/Calculate_F_c' incorporates: * Gain: '/Gain' */ ADM_Integrated_Lo_Calculate_F_c(ADM_Integrated_Logic_ConstB.W_value, 0.017453292519943295 * rtb_Pitch_Angle, &rtb_Product2_a); /* Gain: '/Gain2' incorporates: * Constant: '/Radius' * Gain: '/Gain1' * Gain: '/Rolling_Gain' * MATLAB Function: '/Calculate_F_R' * Product: '/Multiply3' * Sum: '/Required_Brake_Cal' */ rtb_Saturation = (cos(0.017453292519943295 * rtb_Pitch_Angle) * 436.7465753424658 * 0.0 + rtb_Product2_a) * 0.292 * 0.083822296730930432; /* Saturate: '/Saturation' */ if (rtb_Saturation > 60.0) { rtb_Saturation = 60.0; } else if (rtb_Saturation < 0.0) { rtb_Saturation = 0.0; } /* End of Saturate: '/Saturation' */ /* Delay: '/Delay' */ if (ADM_Integrated_Logic_DW.icLoad_e) { ADM_Integrated_Logic_DW.Delay_DSTATE_i = rtb_Saturation; } /* Product: '/delta rise limit' incorporates: * Constant: '/Upper_Torq' * SampleTimeMath: '/sample time' * * About '/sample time': * y = K where K = ( w * Ts ) * */ rtb_Product2_a = 0.016; /* Sum: '/Difference Inputs1' incorporates: * Delay: '/Delay' * * Block description for '/Difference Inputs1': * * Add in CPU */ rtb_Saturation -= ADM_Integrated_Logic_DW.Delay_DSTATE_i; /* Switch: '/Switch2' incorporates: * RelationalOperator: '/LowerRelop1' */ if (!(rtb_Saturation > 0.016)) { /* Switch: '/Switch' incorporates: * RelationalOperator: '/UpperRelop' */ if (rtb_Saturation < -0.16) { rtb_Product2_a = -0.16; } else { rtb_Product2_a = rtb_Saturation; } /* End of Switch: '/Switch' */ } /* End of Switch: '/Switch2' */ /* Sum: '/Difference Inputs2' incorporates: * Delay: '/Delay' * * Block description for '/Difference Inputs2': * * Add in CPU */ rtb_Saturation = rtb_Product2_a + ADM_Integrated_Logic_DW.Delay_DSTATE_i; /* Gain: '/Grade_GAIN' */ rtb_Desired_Torque = 0.8 * rtb_Saturation; /* RelationalOperator: '/Compare' incorporates: * Constant: '/Constant' * Inport: '/GV_BrakeTorqueCommand' */ rtb_Compare_p = (ADM_Integrated_Logic_U.GV_BrakeTorqueCommand >= 100.0); /* Delay: '/Memory' */ rtb_Memory = ADM_Integrated_Logic_DW.Memory_DSTATE; /* MATLAB Function: '/Gear_FUNCTION1' incorporates: * Inport: '/GV_VCU_GearSelStat' * Inport: '/GV_Vx_Fbk' */ if (ADM_Integrated_Logic_U.GV_VCU_GearSelStat == 0.0) { if ((ADM_Integrated_Logic_U.GV_Vx_Fbk <= 0.0) && rtb_Compare_p) { rtb_Memory = 0.0; } } else { switch ((int32_t)rtb_Memory) { case 0: 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))) { rtb_Memory = 2.0; } break; case 1: if ((ADM_Integrated_Logic_U.GV_VCU_GearSelStat == 2.0) && (ADM_Integrated_Logic_U.GV_Vx_Fbk <= 0.0)) { rtb_Memory = 2.0; } break; case 2: if ((ADM_Integrated_Logic_U.GV_Vx_Fbk <= 0.0) && rtb_Compare_p) { if (ADM_Integrated_Logic_U.GV_VCU_GearSelStat == 1.0) { rtb_Memory = 1.0; } else if (ADM_Integrated_Logic_U.GV_VCU_GearSelStat == 3.0) { rtb_Memory = 3.0; } } break; case 3: if ((ADM_Integrated_Logic_U.GV_VCU_GearSelStat == 2.0) && (ADM_Integrated_Logic_U.GV_Vx_Fbk <= 0.0)) { rtb_Memory = 2.0; } break; } } /* End of MATLAB Function: '/Gear_FUNCTION1' */ /* Chart: '/Chart' incorporates: * Constant: '/Constant' * Constant: '/Constant1' * Inport: '/GV_MCU_RPM' */ if (ADM_Integrated_Logic_DW.is_active_c6_ADM_Integrated_Log == 0) { ADM_Integrated_Logic_DW.is_active_c6_ADM_Integrated_Log = 1U; ADM_Integrated_Logic_DW.is_c6_ADM_Integrated_Logic = IN_HAC_OFF; } else if (ADM_Integrated_Logic_DW.is_c6_ADM_Integrated_Logic == IN_HAC_OFF) { if (ADM_Integrated_Logic_U.GV_MCU_RPM < -50.0) { ADM_Integrated_Logic_DW.is_c6_ADM_Integrated_Logic = IN_HAC_ON; } else { /* Outport: '/Debug_HAC_RPM_Decision' */ ADM_Integrated_Logic_Y.Debug_HAC_RPM_Decision = 0.0; } /* case IN_HAC_ON: */ } else if (ADM_Integrated_Logic_U.GV_MCU_RPM > 150.0) { ADM_Integrated_Logic_DW.is_c6_ADM_Integrated_Logic = IN_HAC_OFF; } else { /* Outport: '/Debug_HAC_RPM_Decision' */ ADM_Integrated_Logic_Y.Debug_HAC_RPM_Decision = 1.0; } /* End of Chart: '/Chart' */ /* MATLAB Function: '/HAC_OFF_OK_Func' */ rtb_Brake_Torque_Cmd = 0.0; /* Outport: '/Debug_HAC_FLAG' incorporates: * MATLAB Function: '/HAC_OFF_OK_Func' */ ADM_Integrated_Logic_Y.Debug_HAC_FLAG = 0.0; /* MATLAB Function: '/HAC_OFF_OK_Func' incorporates: * Constant: '/Accel_Cmd ' * Constant: '/Brake_Cmd' * Constant: '/Gear_D' * Inport: '/GV_BrakeTorqueCommand' * Inport: '/GV_MCU_EstTrq' * Outport: '/Debug_HAC_RPM_Decision' * RelationalOperator: '/Relational Operator' * RelationalOperator: '/Relational Operator1' * RelationalOperator: '/Relational Operator2' * Sum: '/HAC_Flags_Add' */ if (((double)(((ADM_Integrated_Logic_U.GV_BrakeTorqueCommand <= 20.0) + (rtb_Vx_Cmd_R <= 2.0)) + (rtb_Memory == 3.0)) + 1.0) + ADM_Integrated_Logic_Y.Debug_HAC_RPM_Decision >= 5.0) { if (ADM_Integrated_Logic_DW.HAC_ON_FLAG == 0.0) { ADM_Integrated_Logic_DW.HAC_ON_Timer = 0.0; } ADM_Integrated_Logic_DW.HAC_ON_FLAG = 1.0; rtb_Brake_Torque_Cmd = 1000.0; ADM_Integrated_Logic_DW.Smoothed_Torque = 1000.0; if (ADM_Integrated_Logic_DW.HAC_ON_Timer < 3.0) { ADM_Integrated_Logic_DW.HAC_Desired_Torque = rtb_Desired_Torque; ADM_Integrated_Logic_DW.HAC_ON_Timer += 0.005; } /* Outport: '/Debug_HAC_FLAG' */ ADM_Integrated_Logic_Y.Debug_HAC_FLAG = 1.0; } else if (ADM_Integrated_Logic_DW.HAC_ON_FLAG == 1.0) { if (ADM_Integrated_Logic_U.GV_MCU_EstTrq >= ADM_Integrated_Logic_DW.HAC_Desired_Torque) { ADM_Integrated_Logic_DW.Smoothed_Torque -= 0.05 * ADM_Integrated_Logic_DW.Smoothed_Torque; if (ADM_Integrated_Logic_DW.Smoothed_Torque < 0.01) { ADM_Integrated_Logic_DW.Smoothed_Torque = 0.0; ADM_Integrated_Logic_DW.HAC_ON_FLAG = 0.0; ADM_Integrated_Logic_DW.HAC_Desired_Torque = 0.0; ADM_Integrated_Logic_DW.HAC_ON_Timer = 0.0; } rtb_Brake_Torque_Cmd = ADM_Integrated_Logic_DW.Smoothed_Torque; } else { rtb_Brake_Torque_Cmd = 1000.0; ADM_Integrated_Logic_DW.Smoothed_Torque = 1000.0; /* Outport: '/Debug_HAC_FLAG' */ ADM_Integrated_Logic_Y.Debug_HAC_FLAG = 1.0; } } /* MATLAB Function: '/MATLAB Function' incorporates: * Inport: '/GV_Vx_Fbk' */ if (fabs(ADM_Integrated_Logic_U.GV_Vx_Fbk) < 0.1) { rtb_AccFlag = 0.0; } else { rtb_AccFlag = 1.0; } /* End of MATLAB Function: '/MATLAB Function' */ /* Product: '/Product2' incorporates: * Constant: '/Constant3' * Product: '/Product' * Product: '/Product1' * Sum: '/Subtract' */ rtb_Product2_a = (0.0 - rtb_Product1 * 1500.0) * rtb_AccFlag; /* SampleTimeMath: '/TSamp' * * About '/TSamp': * y = u * K where K = 1 / ( w * Ts ) * */ rtb_TSamp = rtb_Product2_a * 500.0; /* Sum: '/Subtract1' incorporates: * Constant: '/Constant10' * Constant: '/Constant11' * Constant: '/Constant13' * DiscreteIntegrator: '/Discrete-Time Integrator' * Product: '/D controller' * Product: '/I controller' * Product: '/P controller' * Sum: '/Diff' * UnitDelay: '/UD' * * Block description for '/Diff': * * Add in CPU * * Block description for '/UD': * * Store in Global RAM */ rtb_F_PI = (0.07 * rtb_Product2_a + ADM_Integrated_Logic_DW.DiscreteTimeIntegrator_DSTATE * 0.2) + (rtb_TSamp - ADM_Integrated_Logic_DW.UD_DSTATE) * 0.0; /* Saturate: '/Saturation' */ if (rtb_F_PI > 3525.0) { rtb_y = 3525.0; } else if (rtb_F_PI < -2500.0) { rtb_y = -2500.0; } else { rtb_y = rtb_F_PI; } /* End of Saturate: '/Saturation' */ /* Product: '/Product3' */ rtb_y_h = rtb_AccFlag * rtb_y; /* RateLimiter: '/Rate Limiter' */ rtb_Brake_Saturation = rtb_y_h - ADM_Integrated_Logic_DW.PrevY_b; if (rtb_Brake_Saturation > 2.0) { rtb_y_h = ADM_Integrated_Logic_DW.PrevY_b + 2.0; } else if (rtb_Brake_Saturation < -2.0) { rtb_y_h = ADM_Integrated_Logic_DW.PrevY_b - 2.0; } ADM_Integrated_Logic_DW.PrevY_b = rtb_y_h; /* End of RateLimiter: '/Rate Limiter' */ /* Product: '/Product' incorporates: * MATLAB Function: '/MATLAB Function1' */ rtb_AccFlag = (double)(rtb_Memory == 3.0) * rtb_y_h; /* MATLAB Function: '/MATLAB Function' */ if (!(rtb_AccFlag >= 0.0)) { rtb_AccFlag = 0.0; } /* End of MATLAB Function: '/MATLAB Function' */ /* Gain: '/Brake_GAIN' incorporates: * DiscreteTransferFcn: '/Discrete Transfer Fcn' */ rtb_Integrator_1 = 0.0625 * ADM_Integrated_Logic_DW.DiscreteTransferFcn_states * -80.0; /* MATLAB Function: '/GearCondition_Brake' */ if (rtb_Memory == 1.0) { i = -1; } else { i = (rtb_Memory == 3.0); } /* Saturate: '/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: '/Multiply2' incorporates: * MATLAB Function: '/GearCondition_Brake' * Saturate: '/Saturation' */ rtb_y_h = (double)i * rtb_Integrator_1; /* RateLimiter: '/Brake_Out_RateLimiter' */ rtb_Brake_Saturation = rtb_y_h - ADM_Integrated_Logic_DW.PrevY_o; if (rtb_Brake_Saturation > 3.0) { rtb_y_h = ADM_Integrated_Logic_DW.PrevY_o + 3.0; } else if (rtb_Brake_Saturation < -3.0) { rtb_y_h = ADM_Integrated_Logic_DW.PrevY_o - 3.0; } ADM_Integrated_Logic_DW.PrevY_o = rtb_y_h; /* End of RateLimiter: '/Brake_Out_RateLimiter' */ /* Saturate: '/Brake_Saturation' */ if (rtb_y_h > 1000.0) { rtb_y_h = 1000.0; } else if (rtb_y_h < 0.0) { rtb_y_h = 0.0; } /* End of Saturate: '/Brake_Saturation' */ /* MATLAB Function: '/Emergency_Brake_Func' incorporates: * Inport: '/GV_Vx_Fbk' * Sum: '/Add' * Switch: '/Switch2' */ if (rtb_Compare_l) { rtb_Brake_Saturation = ADM_Integrated_Logic_U.GV_Vx_Fbk * 100.0; } else { rtb_Brake_Saturation = rtb_Brake_Torque_Cmd + rtb_y_h; } /* Saturate: '/Brake_Saturation' incorporates: * MATLAB Function: '/Emergency_Brake_Func' */ if (rtb_Brake_Saturation > 2500.0) { rtb_Brake_Saturation = 2500.0; } else if (rtb_Brake_Saturation < 0.0) { rtb_Brake_Saturation = 0.0; } /* End of Saturate: '/Brake_Saturation' */ /* Outputs for Enabled SubSystem: '/Enabled Subsystem' incorporates: * EnablePort: '/Enable' */ /* MATLAB Function: '/MATLAB Function4' incorporates: * Inport: '/GV_Drive_ACC_Cmd' * Inport: '/GV_MCU_EstTrq' * Inport: '/GV_Vx_Fbk' * MATLAB Function: '/IDB_Fault_Injection' */ if (((rtb_Vx_Cmd_R >= 1.0) || (ADM_Integrated_Logic_U.GV_Drive_ACC_Cmd >= 0.01)) && (ADM_Integrated_Logic_U.GV_MCU_EstTrq >= 10.0) && (rtb_Product1 >= 0.2) && (ADM_Integrated_Logic_U.GV_Vx_Fbk >= 1.5) && (rtb_Brake_Saturation <= 10.0)) { /* MATLAB Function: '/MATLAB Function' incorporates: * Constant: '/Constant9' * Inport: '/GV_IMU_AZ_Val' * MATLAB Function: '/MATLAB Function' */ ADM_Integrated_Logic_DW.M = ADM_Integrated_Logic_U.GV_MCU_EstTrq / ((0.01 * ADM_Integrated_Logic_U.GV_IMU_AZ_Val + ADM_Integrated_Logic_DW.x_hat[1]) * 0.022698910310142498); /* MATLAB Function: '/MATLAB Function3' */ if ((ADM_Integrated_Logic_DW.M > 500.0) && (ADM_Integrated_Logic_DW.M < 2000.0)) { if (fabs(ADM_Integrated_Logic_DW.M - ADM_Integrated_Logic_DW.currentEstMass) / ADM_Integrated_Logic_DW.currentEstMass < 0.2) { rtb_Trig = ADM_Integrated_Logic_DW.currentEstMass; ADM_Integrated_Logic_DW.currentEstMass = 0.98 * ADM_Integrated_Logic_DW.currentEstMass + 0.02 * ADM_Integrated_Logic_DW.M; ADM_Integrated_Logic_DW.isConverged = ((fabs (ADM_Integrated_Logic_DW.currentEstMass - rtb_Trig) < 5.0) || ADM_Integrated_Logic_DW.isConverged); if (ADM_Integrated_Logic_DW.isConverged) { /* Outport: '/GV_VCU_EstMassFlag' */ ADM_Integrated_Logic_Y.GV_VCU_EstMassFlag = 2.0; } else { /* Outport: '/GV_VCU_EstMassFlag' */ ADM_Integrated_Logic_Y.GV_VCU_EstMassFlag = 1.0; } } else { ADM_Integrated_Logic_DW.isConverged = false; /* Outport: '/GV_VCU_EstMassFlag' */ ADM_Integrated_Logic_Y.GV_VCU_EstMassFlag = 1.0; } } else { ADM_Integrated_Logic_DW.isConverged = false; /* Outport: '/GV_VCU_EstMassFlag' */ ADM_Integrated_Logic_Y.GV_VCU_EstMassFlag = 1.0; } } else { /* MATLAB Function: '/MATLAB Function3' */ ADM_Integrated_Logic_DW.isConverged = false; /* Outport: '/GV_VCU_EstMassFlag' incorporates: * MATLAB Function: '/MATLAB Function3' */ ADM_Integrated_Logic_Y.GV_VCU_EstMassFlag = 0.0; } /* End of MATLAB Function: '/MATLAB Function4' */ /* End of Outputs for SubSystem: '/Enabled Subsystem' */ /* Outport: '/GV_VCU_EstMass' incorporates: * MATLAB Function: '/MATLAB Function3' */ ADM_Integrated_Logic_Y.GV_VCU_EstMass = ADM_Integrated_Logic_DW.currentEstMass; /* Outport: '/GV_Brake_Command' incorporates: * MATLAB Function: '/IDB_Fault_Injection' */ ADM_Integrated_Logic_Y.GV_Brake_Command = rtb_Brake_Saturation; /* Outport: '/Target_IDB_Out' incorporates: * MATLAB Function: '/IDB_Fault_Injection' */ ADM_Integrated_Logic_Y.Target_IDB_Out = rtb_Brake_Saturation; /* Outport: '/Debug_CC_Brake_Output' */ ADM_Integrated_Logic_Y.Debug_CC_Brake_Output = rtb_y_h; /* Outport: '/Debug_AccControl_TorqueCmd' incorporates: * Constant: '/Constant' * Constant: '/Constant1' * Product: '/Product1' * Product: '/Product2' */ ADM_Integrated_Logic_Y.Debug_AccControl_TorqueCmd = rtb_AccFlag * 0.270798 / 11.93; /* MATLAB Function: '/Target_RPM' */ if (rtb_Memory == 0.0) { i = 0; } else if (rtb_Memory == 2.0) { i = 0; } else if (rtb_Memory == 1.0) { i = -1; } else { i = (rtb_Memory == 3.0); } rtb_AccFlag = rtb_Vx_Cmd_R * 1000.0 / 3600.0 * 11.93 * 60.0 / 1.7013672006633955 * (double)i; /* End of MATLAB Function: '/Target_RPM' */ /* RateLimiter: '/TargetSpd_RateLimiter' */ rtb_Brake_Saturation = rtb_AccFlag - ADM_Integrated_Logic_DW.PrevY_a; if (rtb_Brake_Saturation > 0.4) { rtb_AccFlag = ADM_Integrated_Logic_DW.PrevY_a + 0.4; } else if (rtb_Brake_Saturation < -0.8) { rtb_AccFlag = ADM_Integrated_Logic_DW.PrevY_a - 0.8; } ADM_Integrated_Logic_DW.PrevY_a = rtb_AccFlag; /* End of RateLimiter: '/TargetSpd_RateLimiter' */ /* Product: '/Product1' incorporates: * Constant: '/Constant' * Gain: '/gain' * Product: '/Product11' * Sum: '/Sum1' * Sum: '/Sum2' * Sum: '/Sum3' * Sum: '/Sum4' * UnitDelay: '/d' * UnitDelay: '/d1' */ rtb_Brake_Saturation = ((rtb_AccFlag + ADM_Integrated_Logic_DW.d1_DSTATE_c) * 0.002 + 0.1041032953945969 * ADM_Integrated_Logic_DW.d_DSTATE_l) / 0.1081032953945969; /* Sum: '/Sum1' incorporates: * Gain: '/gain' * Sum: '/Sum2' * UnitDelay: '/d' * UnitDelay: '/d1' */ rtb_Trig = (rtb_Brake_Saturation - ADM_Integrated_Logic_DW.d_DSTATE_i) * 1000.0 - ADM_Integrated_Logic_DW.d1_DSTATE_p; /* Sum: '/Sum1' incorporates: * Gain: '/gain' * Sum: '/Sum2' * UnitDelay: '/d' * UnitDelay: '/d1' */ rtb_Sum1_j1 = (rtb_Trig - ADM_Integrated_Logic_DW.d_DSTATE_c) * 1000.0 - ADM_Integrated_Logic_DW.d1_DSTATE_h; /* Sum: '/Sum1' incorporates: * Gain: '/gain' * Sum: '/Sum2' * UnitDelay: '/d' * UnitDelay: '/d1' */ rtb_Sum1_d = (rtb_Sum1_j1 - ADM_Integrated_Logic_DW.d_DSTATE_d) * 1000.0 - ADM_Integrated_Logic_DW.d1_DSTATE_l; /* Gain: '/Gain' incorporates: * Constant: '/Constant3' * Constant: '/Constant4' * Constant: '/Constant5' * Product: '/Product' * Product: '/Product1' * Product: '/Product2' * Sum: '/Add5' */ rtb_Gain_c = (((156.8 * rtb_Brake_Saturation + 212.8 * rtb_Trig) + 21.8 * rtb_Sum1_j1) + rtb_Sum1_d) * 9.44822373393802E-6; /* Product: '/Product1' incorporates: * Constant: '/Constant1' * Constant: '/Constant2' * Delay: '/Delay' * Delay: '/Delay1' * Delay: '/Delay2' * Delay: '/Delay3' * Gain: '/gain1' * Gain: '/gain3' * Product: '/x(n), x(n-1), x(n-2)' * Product: '/y(n-1)' * Product: '/y(n-2)' * Sum: '/Sum1' * Sum: '/Sum2' * Sum: '/Sum3' * Sum: '/Sum4' * Sum: '/Sum5' * Sum: '/Sum6' */ rtb_Product1_pd = ((((2.0 * ADM_Integrated_Logic_DW.Delay1_DSTATE + rtb_Gain_c) + ADM_Integrated_Logic_DW.Delay_DSTATE_c[0]) * 0.39478417604357435 - -7.2104316479128512 * ADM_Integrated_Logic_DW.Delay2_DSTATE) - 2.6178993711731877 * ADM_Integrated_Logic_DW.Delay3_DSTATE [0]) / 6.1716689809139611; /* Product: '/Product1' incorporates: * Constant: '/Constant' * Gain: '/gain' * Product: '/Product11' * Sum: '/Sum1' * Sum: '/Sum2' * Sum: '/Sum3' * Sum: '/Sum4' * UnitDelay: '/d' * UnitDelay: '/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: '/Subtract' incorporates: * Inport: '/GV_MCU_RPM' * MultiPortSwitch: '/Multiport Switch1' */ rtb_Error_m = rtb_Brake_Saturation - ADM_Integrated_Logic_U.GV_MCU_RPM; /* Saturate: '/Error_Saturation' incorporates: * MultiPortSwitch: '/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: '/Error_Saturation' */ /* DeadZone: '/Dead Zone' incorporates: * MultiPortSwitch: '/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: '/Sum1' incorporates: * Gain: '/gain' * Sum: '/Sum2' * UnitDelay: '/d' * UnitDelay: '/d1' */ rtb_Error_m = (rtb_Brake_Saturation - ADM_Integrated_Logic_DW.d_DSTATE_ir) * 1000.0 - ADM_Integrated_Logic_DW.d1_DSTATE_o; /* Sum: '/Sum1' incorporates: * Gain: '/gain' * Sum: '/Sum2' * UnitDelay: '/d' * UnitDelay: '/d1' */ rtb_Sum1_f = (rtb_Error_m - ADM_Integrated_Logic_DW.d_DSTATE_m) * 1000.0 - ADM_Integrated_Logic_DW.d1_DSTATE_hm; /* Sum: '/Sum1' incorporates: * Gain: '/gain' * Sum: '/Sum2' * UnitDelay: '/d' * UnitDelay: '/d1' */ rtb_Sum1_cr = (rtb_Sum1_f - ADM_Integrated_Logic_DW.d_DSTATE_mw) * 1000.0 - ADM_Integrated_Logic_DW.d1_DSTATE_g; /* Sum: '/Add3' incorporates: * Constant: '/Constant3' * Constant: '/Constant4' * Constant: '/Constant5' * Gain: '/Gain' * Memory: '/Memory' * Product: '/Product2' * Product: '/Product3' * Product: '/Product4' * Sum: '/Add2' */ rtb_Add3 = (((156.8 * rtb_Brake_Saturation + 212.8 * rtb_Error_m) + 21.8 * rtb_Sum1_f) + rtb_Sum1_cr) * 9.44822373393802E-6 - ADM_Integrated_Logic_DW.Memory_PreviousInput; /* Product: '/Product1' incorporates: * Constant: '/Constant' * Gain: '/gain' * Product: '/Product11' * Sum: '/Sum1' * Sum: '/Sum2' * Sum: '/Sum3' * Sum: '/Sum4' * UnitDelay: '/d' * UnitDelay: '/d1' */ rtb_Product1_k3 = ((rtb_Add3 + ADM_Integrated_Logic_DW.d1_DSTATE_ej) * 0.002 + 0.029830988618379066 * ADM_Integrated_Logic_DW.d_DSTATE_j) / 0.03383098861837907; /* Product: '/Product1' incorporates: * Constant: '/Constant1' * Constant: '/Constant2' * Delay: '/Delay' * Delay: '/Delay1' * Delay: '/Delay2' * Delay: '/Delay3' * Gain: '/gain1' * Gain: '/gain3' * Product: '/x(n), x(n-1), x(n-2)' * Product: '/y(n-1)' * Product: '/y(n-2)' * Sum: '/Sum1' * Sum: '/Sum2' * Sum: '/Sum3' * Sum: '/Sum4' * Sum: '/Sum5' * Sum: '/Sum6' */ rtb_Product1_h = ((((2.0 * ADM_Integrated_Logic_DW.Delay1_DSTATE_c + rtb_Product1_k3) + 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: '/DOB_Gain' incorporates: * Inport: '/GV_Vx_Fbk' */ if (ADM_Integrated_Logic_U.GV_Vx_Fbk < 5.0) { rtb_y_h = 0.0; } else { rtb_y_h = (ADM_Integrated_Logic_U.GV_Vx_Fbk - 5.0) * 0.2; } if (rtb_y_h >= 1.0) { rtb_y_h = 1.0; } /* End of MATLAB Function: '/DOB_Gain' */ /* Product: '/Product1' incorporates: * Constant: '/DOBFlag' * Product: '/Product' */ rtb_Integrator_1 = -(rtb_Product1_h * 0.9); /* Saturate: '/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: '/Subtract2' incorporates: * DeadZone: '/Dead Zone' * MultiPortSwitch: '/Multiport Switch1' * Product: '/Product' * Product: '/Multiply1' * Saturate: '/DOB_Saturation' * Sum: '/Add' */ rtb_Add_e = (rtb_Add_e * 0.044648264844923756 + rtb_Product1_m) + rtb_Integrator_1 * rtb_y_h; /* Saturate: '/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: '/Torq_Saturation' */ /* MATLAB Function: '/Emergency_Motor_Func' incorporates: * Inport: '/GV_IDB_ECU_Fault_Flag' * Inport: '/GV_RCU_ECU_Fault_Flag' * Inport: '/GV_Vx_Fbk' * MATLAB Function: '/Gear_pos_out' */ if (rtb_Compare_l) { if ((ADM_Integrated_Logic_U.GV_IDB_ECU_Fault_Flag == 1.0) && (ADM_Integrated_Logic_U.GV_RCU_ECU_Fault_Flag == 1.0)) { if (ADM_Integrated_Logic_U.GV_Vx_Fbk / 3.6 > 0.5) { rtb_y_h = -80.0; } else { rtb_y_h = 0.0; } } else { rtb_y_h = 0.0; } } else { if (rtb_Memory == 0.0) { /* MATLAB Function: '/Gear_pos_out' */ i = 0; } else if (rtb_Memory == 2.0) { /* MATLAB Function: '/Gear_pos_out' */ i = 0; } else if (rtb_Memory == 1.0) { /* MATLAB Function: '/Gear_pos_out' */ i = -1; } else { /* MATLAB Function: '/Gear_pos_out' */ i = (rtb_Memory == 3.0); } /* Product: '/Multiply' incorporates: * MATLAB Function: '/Gear_pos_out' */ rtb_y_h = (double)i * rtb_Add_e; /* Saturate: '/Saturation' */ if (rtb_y_h < 0.0) { rtb_y_h = 0.0; } /* End of Saturate: '/Saturation' */ } /* End of MATLAB Function: '/Emergency_Motor_Func' */ /* MATLAB Function: '/MCU_Fault_Injection' incorporates: * Inport: '/GV_MCU_Actuator_Fault_Flag' */ if (ADM_Integrated_Logic_U.GV_MCU_Actuator_Fault_Flag == 1.0) { /* Outport: '/GV_Motor_Torque_Cmd' */ ADM_Integrated_Logic_Y.GV_Motor_Torque_Cmd = 0.0; } else { /* Outport: '/GV_Motor_Torque_Cmd' */ ADM_Integrated_Logic_Y.GV_Motor_Torque_Cmd = rtb_y_h; } /* Outport: '/Target_MCU_Out' incorporates: * MATLAB Function: '/MCU_Fault_Injection' */ ADM_Integrated_Logic_Y.Target_MCU_Out = rtb_y_h; /* Sum: '/Sum' */ rtb_F_PI -= rtb_y; /* Outport: '/Debug_HAC_Brake_Output' */ ADM_Integrated_Logic_Y.Debug_HAC_Brake_Output = rtb_Brake_Torque_Cmd; /* Outport: '/GV_Gear_Postion_Out' */ ADM_Integrated_Logic_Y.GV_Gear_Postion_Out = rtb_Memory; /* Outport: '/GV_Hill_Torque_Assist' */ ADM_Integrated_Logic_Y.GV_Hill_Torque_Assist = rtb_Desired_Torque; /* Outport: '/Debug_HAC_Pitch_angle' */ ADM_Integrated_Logic_Y.Debug_HAC_Pitch_angle = rtb_Pitch_Angle; /* MATLAB Function: '/Calculate_F_c1' incorporates: * Gain: '/Gain3' */ ADM_Integrated_Lo_Calculate_F_c(ADM_Integrated_Logic_ConstB.W_Value_for_Brake, 0.017453292519943295 * rtb_Pitch_Angle, &rtb_y); /* Product: '/Product1' incorporates: * Constant: '/Constant' * Gain: '/gain' * Inport: '/GV_IMU_AX_Val' * Product: '/Product11' * Sum: '/Sum1' * Sum: '/Sum2' * Sum: '/Sum3' * Sum: '/Sum4' * UnitDelay: '/d' * UnitDelay: '/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: '/Product1' incorporates: * Constant: '/Constant' * Gain: '/gain' * Inport: '/GV_IMU_AY_Val' * Product: '/Product11' * Sum: '/Sum1' * Sum: '/Sum2' * Sum: '/Sum3' * Sum: '/Sum4' * UnitDelay: '/d' * UnitDelay: '/d1' */ rtb_Brake_Torque_Cmd = ((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: '/Product1' incorporates: * Constant: '/Constant' * Gain: '/gain' * Inport: '/GV_IMU_AZ_Val' * Product: '/Product11' * Sum: '