mirror of
https://github.com/Dev-KATECH/ADM.git
synced 2026-05-17 01:43:59 +09:00
2079 lines
61 KiB
C
2079 lines
61 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 : 14.26
|
|
* Simulink Coder version : 24.2 (R2024b) 21-Jun-2024
|
|
* C/C++ source code generated on : Tue Oct 14 20:08:12 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 <math.h>
|
|
#include <string.h>
|
|
#include <stdbool.h>
|
|
#include <stddef.h>
|
|
|
|
/* Named constants for Chart: '<S12>/Chart' */
|
|
#define IN_HAC_OFF ((uint8_t)1U)
|
|
#define IN_HAC_ON ((uint8_t)2U)
|
|
#define NumBitsPerChar 8U
|
|
|
|
/* 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);
|
|
|
|
/* Forward declaration for local functions */
|
|
static void ADM_Integrated_Logic_mrdiv(const double A[6], const double B[4],
|
|
double Y[6]);
|
|
|
|
#define NOT_USING_NONFINITE_LITERALS 1
|
|
|
|
extern double rtInf;
|
|
extern double rtMinusInf;
|
|
extern double rtNaN;
|
|
extern float rtInfF;
|
|
extern float rtMinusInfF;
|
|
extern float rtNaNF;
|
|
static void rt_InitInfAndNaN(size_t realSize);
|
|
static bool rtIsInf(double value);
|
|
static bool rtIsInfF(float value);
|
|
static bool rtIsNaN(double value);
|
|
static bool rtIsNaNF(float value);
|
|
typedef struct {
|
|
struct {
|
|
uint32_t wordH;
|
|
uint32_t wordL;
|
|
} words;
|
|
} BigEndianIEEEDouble;
|
|
|
|
typedef struct {
|
|
struct {
|
|
uint32_t wordL;
|
|
uint32_t wordH;
|
|
} words;
|
|
} LittleEndianIEEEDouble;
|
|
|
|
typedef struct {
|
|
union {
|
|
float wordLreal;
|
|
uint32_t wordLuint;
|
|
} wordL;
|
|
} IEEESingle;
|
|
|
|
double rtInf;
|
|
double rtMinusInf;
|
|
double rtNaN;
|
|
float rtInfF;
|
|
float rtMinusInfF;
|
|
float rtNaNF;
|
|
static double rtGetInf(void);
|
|
static float rtGetInfF(void);
|
|
static double rtGetMinusInf(void);
|
|
static float rtGetMinusInfF(void);
|
|
static double rtGetNaN(void);
|
|
static float rtGetNaNF(void);
|
|
|
|
/*
|
|
* Initialize the rtInf, rtMinusInf, and rtNaN needed by the
|
|
* generated code. NaN is initialized as non-signaling. Assumes IEEE.
|
|
*/
|
|
static void rt_InitInfAndNaN(size_t realSize)
|
|
{
|
|
(void) (realSize);
|
|
rtNaN = rtGetNaN();
|
|
rtNaNF = rtGetNaNF();
|
|
rtInf = rtGetInf();
|
|
rtInfF = rtGetInfF();
|
|
rtMinusInf = rtGetMinusInf();
|
|
rtMinusInfF = rtGetMinusInfF();
|
|
}
|
|
|
|
/* Test if value is infinite */
|
|
static bool rtIsInf(double value)
|
|
{
|
|
return (bool)((value==rtInf || value==rtMinusInf) ? 1U : 0U);
|
|
}
|
|
|
|
/* Test if single-precision value is infinite */
|
|
static bool rtIsInfF(float value)
|
|
{
|
|
return (bool)(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U);
|
|
}
|
|
|
|
/* Test if value is not a number */
|
|
static bool rtIsNaN(double value)
|
|
{
|
|
bool result = (bool) 0;
|
|
size_t bitsPerReal = sizeof(double) * (NumBitsPerChar);
|
|
if (bitsPerReal == 32U) {
|
|
result = rtIsNaNF((float)value);
|
|
} else {
|
|
union {
|
|
LittleEndianIEEEDouble bitVal;
|
|
double fltVal;
|
|
} tmpVal;
|
|
|
|
tmpVal.fltVal = value;
|
|
result = (bool)((tmpVal.bitVal.words.wordH & 0x7FF00000) == 0x7FF00000 &&
|
|
( (tmpVal.bitVal.words.wordH & 0x000FFFFF) != 0 ||
|
|
(tmpVal.bitVal.words.wordL != 0) ));
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
/* Test if single-precision value is not a number */
|
|
static bool rtIsNaNF(float value)
|
|
{
|
|
IEEESingle tmp;
|
|
tmp.wordL.wordLreal = value;
|
|
return (bool)( (tmp.wordL.wordLuint & 0x7F800000) == 0x7F800000 &&
|
|
(tmp.wordL.wordLuint & 0x007FFFFF) != 0 );
|
|
}
|
|
|
|
/*
|
|
* Initialize rtInf needed by the generated code.
|
|
* Inf is initialized as non-signaling. Assumes IEEE.
|
|
*/
|
|
static double rtGetInf(void)
|
|
{
|
|
size_t bitsPerReal = sizeof(double) * (NumBitsPerChar);
|
|
double inf = 0.0;
|
|
if (bitsPerReal == 32U) {
|
|
inf = rtGetInfF();
|
|
} else {
|
|
union {
|
|
LittleEndianIEEEDouble bitVal;
|
|
double fltVal;
|
|
} tmpVal;
|
|
|
|
tmpVal.bitVal.words.wordH = 0x7FF00000U;
|
|
tmpVal.bitVal.words.wordL = 0x00000000U;
|
|
inf = tmpVal.fltVal;
|
|
}
|
|
|
|
return inf;
|
|
}
|
|
|
|
/*
|
|
* Initialize rtInfF needed by the generated code.
|
|
* Inf is initialized as non-signaling. Assumes IEEE.
|
|
*/
|
|
static float rtGetInfF(void)
|
|
{
|
|
IEEESingle infF;
|
|
infF.wordL.wordLuint = 0x7F800000U;
|
|
return infF.wordL.wordLreal;
|
|
}
|
|
|
|
/*
|
|
* Initialize rtMinusInf needed by the generated code.
|
|
* Inf is initialized as non-signaling. Assumes IEEE.
|
|
*/
|
|
static double rtGetMinusInf(void)
|
|
{
|
|
size_t bitsPerReal = sizeof(double) * (NumBitsPerChar);
|
|
double minf = 0.0;
|
|
if (bitsPerReal == 32U) {
|
|
minf = rtGetMinusInfF();
|
|
} else {
|
|
union {
|
|
LittleEndianIEEEDouble bitVal;
|
|
double fltVal;
|
|
} tmpVal;
|
|
|
|
tmpVal.bitVal.words.wordH = 0xFFF00000U;
|
|
tmpVal.bitVal.words.wordL = 0x00000000U;
|
|
minf = tmpVal.fltVal;
|
|
}
|
|
|
|
return minf;
|
|
}
|
|
|
|
/*
|
|
* Initialize rtMinusInfF needed by the generated code.
|
|
* Inf is initialized as non-signaling. Assumes IEEE.
|
|
*/
|
|
static float rtGetMinusInfF(void)
|
|
{
|
|
IEEESingle minfF;
|
|
minfF.wordL.wordLuint = 0xFF800000U;
|
|
return minfF.wordL.wordLreal;
|
|
}
|
|
|
|
/*
|
|
* Initialize rtNaN needed by the generated code.
|
|
* NaN is initialized as non-signaling. Assumes IEEE.
|
|
*/
|
|
static double rtGetNaN(void)
|
|
{
|
|
size_t bitsPerReal = sizeof(double) * (NumBitsPerChar);
|
|
double nan = 0.0;
|
|
if (bitsPerReal == 32U) {
|
|
nan = rtGetNaNF();
|
|
} else {
|
|
union {
|
|
LittleEndianIEEEDouble bitVal;
|
|
double fltVal;
|
|
} tmpVal;
|
|
|
|
tmpVal.bitVal.words.wordH = 0xFFF80000U;
|
|
tmpVal.bitVal.words.wordL = 0x00000000U;
|
|
nan = tmpVal.fltVal;
|
|
}
|
|
|
|
return nan;
|
|
}
|
|
|
|
/*
|
|
* Initialize rtNaNF needed by the generated code.
|
|
* NaN is initialized as non-signaling. Assumes IEEE.
|
|
*/
|
|
static float rtGetNaNF(void)
|
|
{
|
|
IEEESingle nanF = { { 0.0F } };
|
|
|
|
nanF.wordL.wordLuint = 0xFFC00000U;
|
|
return nanF.wordL.wordLreal;
|
|
}
|
|
|
|
/*
|
|
* Output and update for atomic system:
|
|
* '<S54>/Calculate_F_c'
|
|
* '<S54>/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:
|
|
* '<S1>/MATLAB Function1'
|
|
* '<S1>/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:
|
|
* '<S1>/MATLAB Function2'
|
|
* '<S1>/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;
|
|
}
|
|
}
|
|
|
|
/* Function for MATLAB Function: '<S1>/MATLAB Function' */
|
|
static void ADM_Integrated_Logic_mrdiv(const double A[6], const double B[4],
|
|
double Y[6])
|
|
{
|
|
double a21;
|
|
double a22;
|
|
double a22_tmp;
|
|
int32_t Y_tmp;
|
|
int32_t Y_tmp_0;
|
|
int32_t r1;
|
|
int32_t r2;
|
|
if (fabs(B[1]) > fabs(B[0])) {
|
|
r1 = 1;
|
|
r2 = 0;
|
|
} else {
|
|
r1 = 0;
|
|
r2 = 1;
|
|
}
|
|
|
|
a21 = B[r2] / B[r1];
|
|
a22_tmp = B[r1 + 2];
|
|
a22 = B[r2 + 2] - a22_tmp * a21;
|
|
Y[3 * r1] = A[0] / B[r1];
|
|
Y[3 * r2] = (A[3] - Y[3 * r1] * a22_tmp) / a22;
|
|
Y[3 * r1] -= Y[3 * r2] * a21;
|
|
Y_tmp = 3 * r1 + 1;
|
|
Y[Y_tmp] = A[1] / B[r1];
|
|
Y_tmp_0 = 3 * r2 + 1;
|
|
Y[Y_tmp_0] = (A[4] - Y[Y_tmp] * a22_tmp) / a22;
|
|
Y[Y_tmp] -= Y[Y_tmp_0] * a21;
|
|
Y_tmp = 3 * r1 + 2;
|
|
Y[Y_tmp] = A[2] / B[r1];
|
|
Y_tmp_0 = 3 * r2 + 2;
|
|
Y[Y_tmp_0] = (A[5] - Y[Y_tmp] * a22_tmp) / a22;
|
|
Y[Y_tmp] -= Y[Y_tmp_0] * a21;
|
|
}
|
|
|
|
/* Model step function */
|
|
void ADM_Integrated_Logic_step(void)
|
|
{
|
|
double P[9];
|
|
double P_last[9];
|
|
double P_pred[9];
|
|
double Q[9];
|
|
double b_I_0[9];
|
|
double P_pred_0[6];
|
|
double tmp_1[6];
|
|
double R[4];
|
|
double tmp_2[4];
|
|
double x_hat_pred[3];
|
|
double tmp[2];
|
|
double tmp_0[2];
|
|
double rtb_Add;
|
|
double rtb_Add1;
|
|
double rtb_Add3;
|
|
double rtb_BrakeTorque;
|
|
double rtb_Brake_Torque_Cmd;
|
|
double rtb_Desired_Torque;
|
|
double rtb_Error_m;
|
|
double rtb_Gain;
|
|
double rtb_Gain2_j;
|
|
double rtb_Gain_c;
|
|
double rtb_MotorTorque;
|
|
double rtb_Pitch_Angle;
|
|
double rtb_Product1_c;
|
|
double rtb_Product1_dc;
|
|
double rtb_Product1_h;
|
|
double rtb_Product1_j;
|
|
double rtb_Product1_lo;
|
|
double rtb_Product1_nu;
|
|
double rtb_Product1_p;
|
|
double rtb_RateLimiter;
|
|
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_TSamp;
|
|
double rtb_Target_RPM;
|
|
double rtb_UkYk1_j;
|
|
double rtb_Vx_Cmd_R;
|
|
double rtb_y_c;
|
|
int32_t H;
|
|
int32_t H_0;
|
|
int32_t P_pred_tmp;
|
|
int32_t d_k;
|
|
int32_t idx;
|
|
int32_t j;
|
|
int8_t b_I[9];
|
|
int8_t K_ss_tmp[6];
|
|
int8_t K_ss_tmp_0;
|
|
int8_t K_ss_tmp_1;
|
|
int8_t K_ss_tmp_2;
|
|
bool rtb_Compare;
|
|
bool rtb_Compare_d;
|
|
static const double b[9] = { 1.0, 0.0, 0.0, 0.002, 1.0, 0.0,
|
|
-0.019620000000000002, 0.0, 1.0 };
|
|
|
|
static const int8_t c[6] = { 1, 0, 0, 1, 0, 0 };
|
|
|
|
double rtb_Add1_tmp;
|
|
bool exitg1;
|
|
bool exitg2;
|
|
|
|
/* MATLAB Function: '<S1>/MATLAB Function' */
|
|
if (!ADM_Integrated_Logic_DW.K_ss_not_empty) {
|
|
memcpy(&ADM_Integrated_Logic_DW.A[0], &b[0], 9U * sizeof(double));
|
|
for (idx = 0; idx < 6; idx++) {
|
|
ADM_Integrated_Logic_DW.H[idx] = c[idx];
|
|
}
|
|
|
|
memset(&Q[0], 0, 9U * sizeof(double));
|
|
Q[0] = 0.01;
|
|
Q[4] = 0.1;
|
|
Q[8] = 0.001;
|
|
R[1] = 0.0;
|
|
R[2] = 0.0;
|
|
R[0] = 0.1;
|
|
R[3] = 0.1;
|
|
memset(&P[0], 0, 9U * sizeof(double));
|
|
P[0] = 1.0;
|
|
P[4] = 1.0;
|
|
P[8] = 1.0;
|
|
j = 0;
|
|
exitg1 = false;
|
|
while ((!exitg1) && (j < 100)) {
|
|
memcpy(&P_last[0], &P[0], 9U * sizeof(double));
|
|
for (idx = 0; idx < 3; idx++) {
|
|
rtb_Gain = ADM_Integrated_Logic_DW.A[idx + 3];
|
|
rtb_Switch2 = ADM_Integrated_Logic_DW.A[idx];
|
|
rtb_Gain2_j = ADM_Integrated_Logic_DW.A[idx + 6];
|
|
for (d_k = 0; d_k < 3; d_k++) {
|
|
b_I_0[idx + 3 * d_k] = (P[3 * d_k + 1] * rtb_Gain + P[3 * d_k] *
|
|
rtb_Switch2) + P[3 * d_k + 2] * rtb_Gain2_j;
|
|
}
|
|
|
|
rtb_Add = b_I_0[idx + 3];
|
|
rtb_Gain = b_I_0[idx];
|
|
rtb_Switch2 = b_I_0[idx + 6];
|
|
for (d_k = 0; d_k < 3; d_k++) {
|
|
P_pred_tmp = 3 * d_k + idx;
|
|
P_pred[P_pred_tmp] = ((ADM_Integrated_Logic_DW.A[d_k + 3] * rtb_Add +
|
|
rtb_Gain * ADM_Integrated_Logic_DW.A[d_k]) +
|
|
ADM_Integrated_Logic_DW.A[d_k + 6] * rtb_Switch2)
|
|
+ Q[P_pred_tmp];
|
|
}
|
|
}
|
|
|
|
for (idx = 0; idx < 9; idx++) {
|
|
b_I[idx] = 0;
|
|
}
|
|
|
|
b_I[0] = 1;
|
|
b_I[4] = 1;
|
|
b_I[8] = 1;
|
|
for (idx = 0; idx < 2; idx++) {
|
|
P_pred_tmp = (int32_t)ADM_Integrated_Logic_DW.H[idx + 2];
|
|
H = (int32_t)ADM_Integrated_Logic_DW.H[idx];
|
|
H_0 = (int32_t)ADM_Integrated_Logic_DW.H[idx + 4];
|
|
for (d_k = 0; d_k < 3; d_k++) {
|
|
tmp_1[idx + (d_k << 1)] = (P_pred[3 * d_k + 1] * (double)P_pred_tmp +
|
|
P_pred[3 * d_k] * (double)H) + P_pred[3 * d_k + 2] * (double)H_0;
|
|
}
|
|
}
|
|
|
|
for (idx = 0; idx < 3; idx++) {
|
|
rtb_Switch2 = P_pred[idx + 3];
|
|
rtb_Gain = P_pred[idx];
|
|
rtb_Gain2_j = P_pred[idx + 6];
|
|
for (d_k = 0; d_k < 2; d_k++) {
|
|
P_pred_0[idx + 3 * d_k] = (ADM_Integrated_Logic_DW.H[d_k + 2] *
|
|
rtb_Switch2 + rtb_Gain * ADM_Integrated_Logic_DW.H[d_k]) +
|
|
ADM_Integrated_Logic_DW.H[d_k + 4] * rtb_Gain2_j;
|
|
}
|
|
}
|
|
|
|
for (idx = 0; idx < 2; idx++) {
|
|
rtb_Add = tmp_1[idx + 2];
|
|
rtb_Gain = tmp_1[idx];
|
|
rtb_Switch2 = tmp_1[idx + 4];
|
|
for (d_k = 0; d_k < 2; d_k++) {
|
|
P_pred_tmp = (d_k << 1) + idx;
|
|
tmp_2[P_pred_tmp] = ((ADM_Integrated_Logic_DW.H[d_k + 2] * rtb_Add +
|
|
rtb_Gain * ADM_Integrated_Logic_DW.H[d_k]) +
|
|
ADM_Integrated_Logic_DW.H[d_k + 4] * rtb_Switch2)
|
|
+ R[P_pred_tmp];
|
|
}
|
|
}
|
|
|
|
ADM_Integrated_Logic_mrdiv(P_pred_0, tmp_2, tmp_1);
|
|
for (idx = 0; idx < 3; idx++) {
|
|
rtb_Add = tmp_1[idx + 3];
|
|
rtb_Gain = tmp_1[idx];
|
|
for (d_k = 0; d_k < 3; d_k++) {
|
|
P_pred_tmp = d_k << 1;
|
|
H = 3 * d_k + idx;
|
|
b_I_0[H] = (double)b_I[H] - (ADM_Integrated_Logic_DW.H[P_pred_tmp + 1]
|
|
* rtb_Add + ADM_Integrated_Logic_DW.H[P_pred_tmp] * rtb_Gain);
|
|
}
|
|
|
|
rtb_Gain = b_I_0[idx + 3];
|
|
rtb_Switch2 = b_I_0[idx];
|
|
rtb_Gain2_j = b_I_0[idx + 6];
|
|
for (d_k = 0; d_k < 3; d_k++) {
|
|
P[idx + 3 * d_k] = (P_pred[3 * d_k + 1] * rtb_Gain + P_pred[3 * d_k] *
|
|
rtb_Switch2) + P_pred[3 * d_k + 2] * rtb_Gain2_j;
|
|
}
|
|
}
|
|
|
|
for (idx = 0; idx < 9; idx++) {
|
|
rtb_Gain = P[idx] - P_last[idx];
|
|
P_last[idx] = rtb_Gain;
|
|
P_pred[idx] = fabs(rtb_Gain);
|
|
}
|
|
|
|
if (!rtIsNaN(P_pred[0])) {
|
|
idx = 1;
|
|
} else {
|
|
idx = 0;
|
|
d_k = 2;
|
|
exitg2 = false;
|
|
while ((!exitg2) && (d_k < 10)) {
|
|
if (!rtIsNaN(P_pred[d_k - 1])) {
|
|
idx = d_k;
|
|
exitg2 = true;
|
|
} else {
|
|
d_k++;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (idx == 0) {
|
|
rtb_Gain = P_pred[0];
|
|
} else {
|
|
rtb_Gain = P_pred[idx - 1];
|
|
for (d_k = idx + 1; d_k < 10; d_k++) {
|
|
rtb_Switch2 = P_pred[d_k - 1];
|
|
if (rtb_Gain < rtb_Switch2) {
|
|
rtb_Gain = rtb_Switch2;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (rtb_Gain < 1.0E-6) {
|
|
exitg1 = true;
|
|
} else {
|
|
j++;
|
|
}
|
|
}
|
|
|
|
for (idx = 0; idx < 3; idx++) {
|
|
rtb_Gain = ADM_Integrated_Logic_DW.A[idx + 3];
|
|
rtb_Switch2 = ADM_Integrated_Logic_DW.A[idx];
|
|
rtb_Gain2_j = ADM_Integrated_Logic_DW.A[idx + 6];
|
|
for (d_k = 0; d_k < 3; d_k++) {
|
|
b_I_0[idx + 3 * d_k] = (P[3 * d_k + 1] * rtb_Gain + P[3 * d_k] *
|
|
rtb_Switch2) + P[3 * d_k + 2] * rtb_Gain2_j;
|
|
}
|
|
|
|
rtb_Add = b_I_0[idx + 3];
|
|
rtb_Gain = b_I_0[idx];
|
|
rtb_Switch2 = b_I_0[idx + 6];
|
|
for (d_k = 0; d_k < 3; d_k++) {
|
|
P_pred_tmp = 3 * d_k + idx;
|
|
P_pred[P_pred_tmp] = ((ADM_Integrated_Logic_DW.A[d_k + 3] * rtb_Add +
|
|
rtb_Gain * ADM_Integrated_Logic_DW.A[d_k]) +
|
|
ADM_Integrated_Logic_DW.A[d_k + 6] * rtb_Switch2)
|
|
+ Q[P_pred_tmp];
|
|
}
|
|
}
|
|
|
|
for (idx = 0; idx < 2; idx++) {
|
|
for (d_k = 0; d_k < 3; d_k++) {
|
|
j = (d_k << 1) + idx;
|
|
K_ss_tmp[d_k + 3 * idx] = (int8_t)ADM_Integrated_Logic_DW.H[j];
|
|
tmp_1[j] = (P_pred[3 * d_k + 1] * ADM_Integrated_Logic_DW.H[idx + 2] +
|
|
P_pred[3 * d_k] * ADM_Integrated_Logic_DW.H[idx]) + P_pred[3
|
|
* d_k + 2] * ADM_Integrated_Logic_DW.H[idx + 4];
|
|
}
|
|
|
|
K_ss_tmp_0 = K_ss_tmp[3 * idx + 1];
|
|
K_ss_tmp_1 = K_ss_tmp[3 * idx];
|
|
K_ss_tmp_2 = K_ss_tmp[3 * idx + 2];
|
|
for (d_k = 0; d_k < 3; d_k++) {
|
|
P_pred_0[d_k + 3 * idx] = (P_pred[d_k + 3] * (double)K_ss_tmp_0 +
|
|
(double)K_ss_tmp_1 * P_pred[d_k]) + P_pred[d_k + 6] * (double)
|
|
K_ss_tmp_2;
|
|
}
|
|
}
|
|
|
|
for (idx = 0; idx < 2; idx++) {
|
|
rtb_Add = tmp_1[idx + 2];
|
|
rtb_Gain = tmp_1[idx];
|
|
rtb_Switch2 = tmp_1[idx + 4];
|
|
for (d_k = 0; d_k < 2; d_k++) {
|
|
P_pred_tmp = (d_k << 1) + idx;
|
|
tmp_2[P_pred_tmp] = (((double)K_ss_tmp[3 * d_k + 1] * rtb_Add + (double)
|
|
K_ss_tmp[3 * d_k] * rtb_Gain) + (double)K_ss_tmp[3
|
|
* d_k + 2] * rtb_Switch2) + R[P_pred_tmp];
|
|
}
|
|
}
|
|
|
|
ADM_Integrated_Logic_mrdiv(P_pred_0, tmp_2, ADM_Integrated_Logic_DW.K_ss);
|
|
ADM_Integrated_Logic_DW.K_ss_not_empty = true;
|
|
ADM_Integrated_Logic_DW.x_hat[0] = 0.0;
|
|
ADM_Integrated_Logic_DW.x_hat[1] = 0.0;
|
|
ADM_Integrated_Logic_DW.x_hat[2] = 0.0;
|
|
}
|
|
|
|
rtb_Gain = ADM_Integrated_Logic_DW.x_hat[1];
|
|
rtb_Switch2 = ADM_Integrated_Logic_DW.x_hat[0];
|
|
rtb_Gain2_j = ADM_Integrated_Logic_DW.x_hat[2];
|
|
for (idx = 0; idx < 3; idx++) {
|
|
x_hat_pred[idx] = (ADM_Integrated_Logic_DW.A[idx + 3] * rtb_Gain +
|
|
ADM_Integrated_Logic_DW.A[idx] * rtb_Switch2) +
|
|
ADM_Integrated_Logic_DW.A[idx + 6] * rtb_Gain2_j;
|
|
}
|
|
|
|
/* SignalConversion generated from: '<S15>/ SFunction ' incorporates:
|
|
* Inport: '<Root>/GV_IMU_AX_Val'
|
|
* Inport: '<Root>/GV_Vx_Fbk'
|
|
* MATLAB Function: '<S1>/MATLAB Function'
|
|
*/
|
|
tmp[0] = ADM_Integrated_Logic_U.GV_Vx_Fbk;
|
|
tmp[1] = ADM_Integrated_Logic_U.GV_IMU_AX_Val;
|
|
|
|
/* MATLAB Function: '<S1>/MATLAB Function' */
|
|
rtb_Gain = x_hat_pred[1];
|
|
rtb_Switch2 = x_hat_pred[0];
|
|
rtb_Gain2_j = x_hat_pred[2];
|
|
for (idx = 0; idx < 2; idx++) {
|
|
tmp_0[idx] = tmp[idx] - ((ADM_Integrated_Logic_DW.H[idx + 2] * rtb_Gain +
|
|
ADM_Integrated_Logic_DW.H[idx] * rtb_Switch2) +
|
|
ADM_Integrated_Logic_DW.H[idx + 4] * rtb_Gain2_j);
|
|
}
|
|
|
|
rtb_Add = tmp_0[1];
|
|
rtb_Gain = tmp_0[0];
|
|
for (idx = 0; idx < 3; idx++) {
|
|
ADM_Integrated_Logic_DW.x_hat[idx] = (ADM_Integrated_Logic_DW.K_ss[idx + 3] *
|
|
rtb_Add + ADM_Integrated_Logic_DW.K_ss[idx] * rtb_Gain) + x_hat_pred[idx];
|
|
}
|
|
|
|
/* RelationalOperator: '<S4>/Compare' incorporates:
|
|
* Constant: '<S4>/Constant'
|
|
* Inport: '<Root>/GV_Drive_Mode'
|
|
*/
|
|
rtb_Compare = (ADM_Integrated_Logic_U.GV_Drive_Mode == 2.0);
|
|
|
|
/* Gain: '<S2>/Gain' incorporates:
|
|
* Inport: '<Root>/GV_Drive_ACC_Cmd'
|
|
*/
|
|
rtb_Gain = 9.80665 * ADM_Integrated_Logic_U.GV_Drive_ACC_Cmd;
|
|
|
|
/* Switch: '<S27>/Switch2' incorporates:
|
|
* Gain: '<S2>/Gain2'
|
|
* Inport: '<Root>/GV_VCU_Acc_Limit'
|
|
* RelationalOperator: '<S27>/LowerRelop1'
|
|
* RelationalOperator: '<S27>/UpperRelop'
|
|
* Switch: '<S27>/Switch'
|
|
*/
|
|
if (rtb_Gain > ADM_Integrated_Logic_U.GV_VCU_Acc_Limit) {
|
|
rtb_Switch2 = ADM_Integrated_Logic_U.GV_VCU_Acc_Limit;
|
|
} else if (rtb_Gain < -ADM_Integrated_Logic_U.GV_VCU_Acc_Limit) {
|
|
/* Switch: '<S27>/Switch' incorporates:
|
|
* Gain: '<S2>/Gain2'
|
|
*/
|
|
rtb_Switch2 = -ADM_Integrated_Logic_U.GV_VCU_Acc_Limit;
|
|
} else {
|
|
rtb_Switch2 = rtb_Gain;
|
|
}
|
|
|
|
/* End of Switch: '<S27>/Switch2' */
|
|
|
|
/* Gain: '<S25>/Gain' */
|
|
rtb_Switch2 *= 1300.0;
|
|
|
|
/* Product: '<S14>/Product1' incorporates:
|
|
* Constant: '<S14>/Constant'
|
|
* Gain: '<S14>/gain'
|
|
* Inport: '<Root>/GV_IMU_AX_Val'
|
|
* Product: '<S14>/Product11'
|
|
* Sum: '<S14>/Sum1'
|
|
* Sum: '<S14>/Sum2'
|
|
* Sum: '<S14>/Sum3'
|
|
* Sum: '<S14>/Sum4'
|
|
* UnitDelay: '<S14>/d'
|
|
* UnitDelay: '<S14>/d1'
|
|
*/
|
|
rtb_Gain = ((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;
|
|
|
|
/* Sum: '<S25>/Subtract' incorporates:
|
|
* Gain: '<S25>/Gain1'
|
|
* Gain: '<S2>/Gain1'
|
|
*/
|
|
rtb_Gain2_j = rtb_Switch2 - 9.80665 * rtb_Gain * 1300.0;
|
|
|
|
/* SampleTimeMath: '<S28>/TSamp'
|
|
*
|
|
* About '<S28>/TSamp':
|
|
* y = u * K where K = 1 / ( w * Ts )
|
|
* */
|
|
rtb_TSamp = rtb_Gain2_j * 500.0;
|
|
|
|
/* Sum: '<S25>/Sum1' incorporates:
|
|
* Constant: '<S1>/Constant10'
|
|
* Constant: '<S1>/Constant11'
|
|
* Constant: '<S1>/Constant13'
|
|
* DiscreteIntegrator: '<S25>/Discrete-Time Integrator'
|
|
* Gain: '<S25>/Gain2'
|
|
* Product: '<S25>/D controller'
|
|
* Product: '<S25>/I controller'
|
|
* Product: '<S25>/P controller'
|
|
* Sum: '<S25>/Subtract1'
|
|
* Sum: '<S28>/Diff'
|
|
* UnitDelay: '<S28>/UD'
|
|
*
|
|
* Block description for '<S28>/Diff':
|
|
*
|
|
* Add in CPU
|
|
*
|
|
* Block description for '<S28>/UD':
|
|
*
|
|
* Store in Global RAM
|
|
*/
|
|
rtb_Switch2 = ((0.05 * rtb_Gain2_j +
|
|
ADM_Integrated_Logic_DW.DiscreteTimeIntegrator_DSTATE * 0.1) +
|
|
(rtb_TSamp - ADM_Integrated_Logic_DW.UD_DSTATE) * 0.0) + 0.0 *
|
|
rtb_Switch2;
|
|
|
|
/* RateLimiter: '<S2>/Rate Limiter' */
|
|
rtb_Product1_c = rtb_Switch2 - ADM_Integrated_Logic_DW.PrevY;
|
|
if (rtb_Product1_c > 1.0) {
|
|
rtb_RateLimiter = ADM_Integrated_Logic_DW.PrevY + 1.0;
|
|
} else if (rtb_Product1_c < -0.3) {
|
|
rtb_RateLimiter = ADM_Integrated_Logic_DW.PrevY - 0.3;
|
|
} else {
|
|
rtb_RateLimiter = rtb_Switch2;
|
|
}
|
|
|
|
ADM_Integrated_Logic_DW.PrevY = rtb_RateLimiter;
|
|
|
|
/* End of RateLimiter: '<S2>/Rate Limiter' */
|
|
|
|
/* MATLAB Function: '<S2>/MATLAB Function' */
|
|
if (rtb_RateLimiter >= 0.0) {
|
|
rtb_MotorTorque = rtb_RateLimiter;
|
|
rtb_BrakeTorque = 0.0;
|
|
} else {
|
|
rtb_MotorTorque = 0.0;
|
|
rtb_BrakeTorque = -rtb_RateLimiter;
|
|
}
|
|
|
|
/* End of MATLAB Function: '<S2>/MATLAB Function' */
|
|
|
|
/* RelationalOperator: '<S50>/Compare' incorporates:
|
|
* Constant: '<S50>/Constant'
|
|
* Inport: '<Root>/GV_BrakeTorqueCommand'
|
|
*/
|
|
rtb_Compare_d = (ADM_Integrated_Logic_U.GV_BrakeTorqueCommand >= 100.0);
|
|
|
|
/* Delay: '<S11>/Memory' */
|
|
rtb_RateLimiter = ADM_Integrated_Logic_DW.Memory_DSTATE;
|
|
|
|
/* MATLAB Function: '<S11>/Gear_FUNCTION1' incorporates:
|
|
* Inport: '<Root>/GV_VCU_GearSelStat'
|
|
* Inport: '<Root>/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_d) {
|
|
rtb_RateLimiter = 0.0;
|
|
}
|
|
} else {
|
|
switch ((int32_t)rtb_RateLimiter) {
|
|
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_RateLimiter = 2.0;
|
|
}
|
|
break;
|
|
|
|
case 1:
|
|
if (ADM_Integrated_Logic_U.GV_VCU_GearSelStat == 2.0) {
|
|
rtb_RateLimiter = 2.0;
|
|
}
|
|
break;
|
|
|
|
case 2:
|
|
if ((ADM_Integrated_Logic_U.GV_Vx_Fbk <= 0.0) && rtb_Compare_d) {
|
|
if (ADM_Integrated_Logic_U.GV_VCU_GearSelStat == 1.0) {
|
|
rtb_RateLimiter = 1.0;
|
|
} else if (ADM_Integrated_Logic_U.GV_VCU_GearSelStat == 3.0) {
|
|
rtb_RateLimiter = 3.0;
|
|
}
|
|
}
|
|
break;
|
|
|
|
case 3:
|
|
if (ADM_Integrated_Logic_U.GV_VCU_GearSelStat == 2.0) {
|
|
rtb_RateLimiter = 2.0;
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
|
|
/* End of MATLAB Function: '<S11>/Gear_FUNCTION1' */
|
|
|
|
/* MATLAB Function: '<S24>/Vx_OutPut_Function' incorporates:
|
|
* Constant: '<S69>/Constant'
|
|
* Inport: '<Root>/GV_BrakeTorqueCommand'
|
|
* Inport: '<Root>/GV_Vx_Command'
|
|
* Inport: '<Root>/GV_Vx_Limit'
|
|
* RelationalOperator: '<S69>/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 = ADM_Integrated_Logic_U.GV_Vx_Limit;
|
|
} else {
|
|
rtb_Add = ADM_Integrated_Logic_U.GV_Vx_Command;
|
|
}
|
|
} else {
|
|
rtb_Add = 0.0;
|
|
}
|
|
|
|
/* Product: '<S24>/Product' incorporates:
|
|
* MATLAB Function: '<S24>/Vx_OutPut_Function'
|
|
* Switch: '<S24>/Switch'
|
|
*/
|
|
rtb_Switch2 = rtb_Add * (double)!rtb_Compare;
|
|
|
|
/* RateLimiter: '<S24>/Input_Vx_RateLimiter' */
|
|
rtb_Product1_c = rtb_Switch2 - ADM_Integrated_Logic_DW.PrevY_a;
|
|
if (rtb_Product1_c > 0.004) {
|
|
rtb_Vx_Cmd_R = ADM_Integrated_Logic_DW.PrevY_a + 0.004;
|
|
} else if (rtb_Product1_c < -0.008) {
|
|
rtb_Vx_Cmd_R = ADM_Integrated_Logic_DW.PrevY_a - 0.008;
|
|
} else {
|
|
rtb_Vx_Cmd_R = rtb_Switch2;
|
|
}
|
|
|
|
ADM_Integrated_Logic_DW.PrevY_a = rtb_Vx_Cmd_R;
|
|
|
|
/* End of RateLimiter: '<S24>/Input_Vx_RateLimiter' */
|
|
|
|
/* MATLAB Function: '<S7>/Target_RPM' */
|
|
if (rtb_RateLimiter == 0.0) {
|
|
idx = 0;
|
|
} else if (rtb_RateLimiter == 2.0) {
|
|
idx = 0;
|
|
} else if (rtb_RateLimiter == 1.0) {
|
|
idx = -1;
|
|
} else {
|
|
idx = (rtb_RateLimiter == 3.0);
|
|
}
|
|
|
|
rtb_Target_RPM = rtb_Vx_Cmd_R * 1000.0 / 3600.0 * 11.93 * 60.0 /
|
|
1.7013672006633955 * (double)idx;
|
|
|
|
/* End of MATLAB Function: '<S7>/Target_RPM' */
|
|
|
|
/* RateLimiter: '<S7>/TargetSpd_RateLimiter' */
|
|
rtb_Product1_c = rtb_Target_RPM - ADM_Integrated_Logic_DW.PrevY_ak;
|
|
if (rtb_Product1_c > 0.4) {
|
|
rtb_Target_RPM = ADM_Integrated_Logic_DW.PrevY_ak + 0.4;
|
|
} else if (rtb_Product1_c < -0.8) {
|
|
rtb_Target_RPM = ADM_Integrated_Logic_DW.PrevY_ak - 0.8;
|
|
}
|
|
|
|
ADM_Integrated_Logic_DW.PrevY_ak = rtb_Target_RPM;
|
|
|
|
/* End of RateLimiter: '<S7>/TargetSpd_RateLimiter' */
|
|
|
|
/* 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_j = ((rtb_Target_RPM + ADM_Integrated_Logic_DW.d1_DSTATE_c) *
|
|
0.002 + 0.1041032953945969 *
|
|
ADM_Integrated_Logic_DW.d_DSTATE_l) / 0.1081032953945969;
|
|
|
|
/* Sum: '<S44>/Sum1' incorporates:
|
|
* Gain: '<S44>/gain'
|
|
* Sum: '<S44>/Sum2'
|
|
* UnitDelay: '<S44>/d'
|
|
* UnitDelay: '<S44>/d1'
|
|
*/
|
|
rtb_Sum1_i2 = (rtb_Product1_j - ADM_Integrated_Logic_DW.d_DSTATE_i) * 1000.0 -
|
|
ADM_Integrated_Logic_DW.d1_DSTATE_p;
|
|
|
|
/* Sum: '<S45>/Sum1' incorporates:
|
|
* Gain: '<S45>/gain'
|
|
* Sum: '<S45>/Sum2'
|
|
* UnitDelay: '<S45>/d'
|
|
* UnitDelay: '<S45>/d1'
|
|
*/
|
|
rtb_Sum1_o1 = (rtb_Sum1_i2 - ADM_Integrated_Logic_DW.d_DSTATE_c) * 1000.0 -
|
|
ADM_Integrated_Logic_DW.d1_DSTATE_h;
|
|
|
|
/* Sum: '<S46>/Sum1' incorporates:
|
|
* Gain: '<S46>/gain'
|
|
* Sum: '<S46>/Sum2'
|
|
* UnitDelay: '<S46>/d'
|
|
* UnitDelay: '<S46>/d1'
|
|
*/
|
|
rtb_Sum1_lm = (rtb_Sum1_o1 - ADM_Integrated_Logic_DW.d_DSTATE_d) * 1000.0 -
|
|
ADM_Integrated_Logic_DW.d1_DSTATE_l;
|
|
|
|
/* Gain: '<S42>/Gain' incorporates:
|
|
* Constant: '<S42>/Constant3'
|
|
* Constant: '<S42>/Constant4'
|
|
* Constant: '<S42>/Constant5'
|
|
* Product: '<S42>/Product'
|
|
* Product: '<S42>/Product1'
|
|
* Product: '<S42>/Product2'
|
|
* Sum: '<S42>/Add5'
|
|
*/
|
|
rtb_Gain_c = (((156.8 * rtb_Product1_j + 212.8 * rtb_Sum1_i2) + 21.8 *
|
|
rtb_Sum1_o1) + rtb_Sum1_lm) * 9.44822373393802E-6;
|
|
|
|
/* Product: '<S48>/Product1' incorporates:
|
|
* Constant: '<S48>/Constant1'
|
|
* Constant: '<S48>/Constant2'
|
|
* Delay: '<S48>/Delay'
|
|
* Delay: '<S48>/Delay1'
|
|
* Delay: '<S48>/Delay2'
|
|
* Delay: '<S48>/Delay3'
|
|
* Gain: '<S48>/gain1'
|
|
* Gain: '<S48>/gain3'
|
|
* Product: '<S48>/x(n), x(n-1), x(n-2)'
|
|
* Product: '<S48>/y(n-1)'
|
|
* Product: '<S48>/y(n-2)'
|
|
* Sum: '<S48>/Sum1'
|
|
* Sum: '<S48>/Sum2'
|
|
* Sum: '<S48>/Sum3'
|
|
* Sum: '<S48>/Sum4'
|
|
* Sum: '<S48>/Sum5'
|
|
* Sum: '<S48>/Sum6'
|
|
*/
|
|
rtb_Product1_dc = ((((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: '<S47>/Product1' incorporates:
|
|
* Constant: '<S47>/Constant'
|
|
* Gain: '<S47>/gain'
|
|
* Product: '<S47>/Product11'
|
|
* Sum: '<S47>/Sum1'
|
|
* Sum: '<S47>/Sum2'
|
|
* Sum: '<S47>/Sum3'
|
|
* Sum: '<S47>/Sum4'
|
|
* UnitDelay: '<S47>/d'
|
|
* UnitDelay: '<S47>/d1'
|
|
*/
|
|
rtb_Product1_h = ((rtb_Product1_dc + ADM_Integrated_Logic_DW.d1_DSTATE_e) *
|
|
0.002 + 0.00861032953945969 *
|
|
ADM_Integrated_Logic_DW.d_DSTATE_ij) / 0.01261032953945969;
|
|
|
|
/* Sum: '<S7>/Subtract' incorporates:
|
|
* Inport: '<Root>/GV_MCU_RPM'
|
|
* MultiPortSwitch: '<S41>/Multiport Switch1'
|
|
*/
|
|
rtb_Error_m = rtb_Product1_j - ADM_Integrated_Logic_U.GV_MCU_RPM;
|
|
|
|
/* Saturate: '<S7>/Error_Saturation' incorporates:
|
|
* MultiPortSwitch: '<S41>/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: '<S7>/Error_Saturation' */
|
|
|
|
/* DeadZone: '<S31>/Dead Zone' incorporates:
|
|
* MultiPortSwitch: '<S41>/Multiport Switch1'
|
|
*/
|
|
if (rtb_Error_m > 50.0) {
|
|
rtb_Add = rtb_Error_m - 50.0;
|
|
} else if (rtb_Error_m >= -50.0) {
|
|
rtb_Add = 0.0;
|
|
} else {
|
|
rtb_Add = rtb_Error_m - -50.0;
|
|
}
|
|
|
|
/* Sum: '<S36>/Sum1' incorporates:
|
|
* Gain: '<S36>/gain'
|
|
* Sum: '<S36>/Sum2'
|
|
* UnitDelay: '<S36>/d'
|
|
* UnitDelay: '<S36>/d1'
|
|
*/
|
|
rtb_Error_m = (rtb_Product1_j - ADM_Integrated_Logic_DW.d_DSTATE_ir) * 1000.0
|
|
- ADM_Integrated_Logic_DW.d1_DSTATE_o;
|
|
|
|
/* Sum: '<S37>/Sum1' incorporates:
|
|
* Gain: '<S37>/gain'
|
|
* Sum: '<S37>/Sum2'
|
|
* UnitDelay: '<S37>/d'
|
|
* UnitDelay: '<S37>/d1'
|
|
*/
|
|
rtb_Sum1_aj = (rtb_Error_m - ADM_Integrated_Logic_DW.d_DSTATE_m) * 1000.0 -
|
|
ADM_Integrated_Logic_DW.d1_DSTATE_hm;
|
|
|
|
/* Sum: '<S38>/Sum1' incorporates:
|
|
* Gain: '<S38>/gain'
|
|
* Sum: '<S38>/Sum2'
|
|
* UnitDelay: '<S38>/d'
|
|
* UnitDelay: '<S38>/d1'
|
|
*/
|
|
rtb_Sum1_p = (rtb_Sum1_aj - ADM_Integrated_Logic_DW.d_DSTATE_mw) * 1000.0 -
|
|
ADM_Integrated_Logic_DW.d1_DSTATE_g;
|
|
|
|
/* Sum: '<S29>/Add3' incorporates:
|
|
* Constant: '<S29>/Constant3'
|
|
* Constant: '<S29>/Constant4'
|
|
* Constant: '<S29>/Constant5'
|
|
* Gain: '<S29>/Gain'
|
|
* Memory: '<S7>/Memory'
|
|
* Product: '<S29>/Product2'
|
|
* Product: '<S29>/Product3'
|
|
* Product: '<S29>/Product4'
|
|
* Sum: '<S29>/Add2'
|
|
*/
|
|
rtb_Add3 = (((156.8 * rtb_Product1_j + 212.8 * rtb_Error_m) + 21.8 *
|
|
rtb_Sum1_aj) + rtb_Sum1_p) * 9.44822373393802E-6 -
|
|
ADM_Integrated_Logic_DW.Memory_PreviousInput;
|
|
|
|
/* Product: '<S39>/Product1' incorporates:
|
|
* Constant: '<S39>/Constant'
|
|
* Gain: '<S39>/gain'
|
|
* Product: '<S39>/Product11'
|
|
* Sum: '<S39>/Sum1'
|
|
* Sum: '<S39>/Sum2'
|
|
* Sum: '<S39>/Sum3'
|
|
* Sum: '<S39>/Sum4'
|
|
* UnitDelay: '<S39>/d'
|
|
* UnitDelay: '<S39>/d1'
|
|
*/
|
|
rtb_Product1_nu = ((rtb_Add3 + ADM_Integrated_Logic_DW.d1_DSTATE_ej) * 0.002 +
|
|
0.029830988618379066 * ADM_Integrated_Logic_DW.d_DSTATE_j) /
|
|
0.03383098861837907;
|
|
|
|
/* Product: '<S40>/Product1' incorporates:
|
|
* Constant: '<S40>/Constant1'
|
|
* Constant: '<S40>/Constant2'
|
|
* Delay: '<S40>/Delay'
|
|
* Delay: '<S40>/Delay1'
|
|
* Delay: '<S40>/Delay2'
|
|
* Delay: '<S40>/Delay3'
|
|
* Gain: '<S40>/gain1'
|
|
* Gain: '<S40>/gain3'
|
|
* Product: '<S40>/x(n), x(n-1), x(n-2)'
|
|
* Product: '<S40>/y(n-1)'
|
|
* Product: '<S40>/y(n-2)'
|
|
* Sum: '<S40>/Sum1'
|
|
* Sum: '<S40>/Sum2'
|
|
* Sum: '<S40>/Sum3'
|
|
* Sum: '<S40>/Sum4'
|
|
* Sum: '<S40>/Sum5'
|
|
* Sum: '<S40>/Sum6'
|
|
*/
|
|
rtb_Product1_p = ((((2.0 * ADM_Integrated_Logic_DW.Delay1_DSTATE_c +
|
|
rtb_Product1_nu) +
|
|
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: '<S7>/DOB_Gain' incorporates:
|
|
* Inport: '<Root>/GV_Vx_Fbk'
|
|
*/
|
|
if (ADM_Integrated_Logic_U.GV_Vx_Fbk < 5.0) {
|
|
rtb_Switch2 = 0.0;
|
|
} else {
|
|
rtb_Switch2 = (ADM_Integrated_Logic_U.GV_Vx_Fbk - 5.0) * 0.2;
|
|
}
|
|
|
|
if (rtb_Switch2 >= 1.0) {
|
|
rtb_Switch2 = 1.0;
|
|
}
|
|
|
|
/* End of MATLAB Function: '<S7>/DOB_Gain' */
|
|
|
|
/* Product: '<S7>/Product1' incorporates:
|
|
* Constant: '<S7>/DOBFlag'
|
|
* Product: '<S7>/Product'
|
|
*/
|
|
rtb_Product1_c = -(rtb_Product1_p * 0.9);
|
|
|
|
/* Saturate: '<S7>/DOB_Saturation' */
|
|
if (rtb_Product1_c > 30.0) {
|
|
rtb_Product1_c = 30.0;
|
|
} else if (rtb_Product1_c < -30.0) {
|
|
rtb_Product1_c = -30.0;
|
|
}
|
|
|
|
/* Sum: '<S7>/Subtract2' incorporates:
|
|
* DeadZone: '<S31>/Dead Zone'
|
|
* MultiPortSwitch: '<S41>/Multiport Switch1'
|
|
* Product: '<S43>/Product'
|
|
* Product: '<S7>/Multiply1'
|
|
* Saturate: '<S7>/DOB_Saturation'
|
|
* Sum: '<S31>/Add'
|
|
*/
|
|
rtb_Add = (rtb_Add * 0.044648264844923756 + rtb_Product1_h) + rtb_Product1_c *
|
|
rtb_Switch2;
|
|
|
|
/* Saturate: '<S7>/Torq_Saturation' */
|
|
if (rtb_Add > 80.0) {
|
|
rtb_Add = 80.0;
|
|
} else if (rtb_Add < -80.0) {
|
|
rtb_Add = -80.0;
|
|
}
|
|
|
|
/* End of Saturate: '<S7>/Torq_Saturation' */
|
|
|
|
/* Switch: '<S1>/Switch1' incorporates:
|
|
* Constant: '<S2>/Constant'
|
|
* Inport: '<Root>/GV_VCU_AccControlFlag'
|
|
* MATLAB Function: '<S7>/Gear_pos_out'
|
|
* Product: '<S2>/Product1'
|
|
* Product: '<S7>/Multiply'
|
|
*/
|
|
if (ADM_Integrated_Logic_U.GV_VCU_AccControlFlag > 0.0) {
|
|
rtb_Pitch_Angle = rtb_MotorTorque * 0.270798;
|
|
} else {
|
|
if (rtb_RateLimiter == 0.0) {
|
|
/* MATLAB Function: '<S7>/Gear_pos_out' */
|
|
idx = 0;
|
|
} else if (rtb_RateLimiter == 2.0) {
|
|
/* MATLAB Function: '<S7>/Gear_pos_out' */
|
|
idx = 0;
|
|
} else if (rtb_RateLimiter == 1.0) {
|
|
/* MATLAB Function: '<S7>/Gear_pos_out' */
|
|
idx = -1;
|
|
} else {
|
|
/* MATLAB Function: '<S7>/Gear_pos_out' */
|
|
idx = (rtb_RateLimiter == 3.0);
|
|
}
|
|
|
|
rtb_Pitch_Angle = (double)idx * rtb_Add;
|
|
}
|
|
|
|
/* End of Switch: '<S1>/Switch1' */
|
|
|
|
/* MATLAB Function: '<S1>/Emergency_Motor_Func' incorporates:
|
|
* Inport: '<Root>/GV_IDB_ECU_Fault_Flag'
|
|
* Inport: '<Root>/GV_RCU_ECU_Fault_Flag'
|
|
* Inport: '<Root>/GV_Vx_Fbk'
|
|
* Saturate: '<S1>/Saturation'
|
|
*/
|
|
if (rtb_Compare) {
|
|
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_MotorTorque = -80.0;
|
|
} else {
|
|
rtb_MotorTorque = 0.0;
|
|
}
|
|
} else {
|
|
rtb_MotorTorque = 0.0;
|
|
}
|
|
} else if (rtb_Pitch_Angle > 80.0) {
|
|
/* Saturate: '<S1>/Saturation' */
|
|
rtb_MotorTorque = 80.0;
|
|
} else if (rtb_Pitch_Angle < 0.0) {
|
|
/* Saturate: '<S1>/Saturation' */
|
|
rtb_MotorTorque = 0.0;
|
|
} else {
|
|
/* Saturate: '<S1>/Saturation' */
|
|
rtb_MotorTorque = rtb_Pitch_Angle;
|
|
}
|
|
|
|
/* End of MATLAB Function: '<S1>/Emergency_Motor_Func' */
|
|
|
|
/* MATLAB Function: '<S1>/MCU_Fault_Injection' incorporates:
|
|
* Inport: '<Root>/GV_MCU_Actuator_Fault_Flag'
|
|
*/
|
|
if (ADM_Integrated_Logic_U.GV_MCU_Actuator_Fault_Flag == 1.0) {
|
|
rtb_Switch2 = 0.0;
|
|
} else {
|
|
rtb_Switch2 = rtb_MotorTorque;
|
|
}
|
|
|
|
/* Logic: '<S1>/Logical Operator' incorporates:
|
|
* Constant: '<S5>/Constant'
|
|
* Constant: '<S6>/Constant'
|
|
* Inport: '<Root>/GV_IMU_AX_Val'
|
|
* Inport: '<Root>/GV_Vx_Fbk'
|
|
* RelationalOperator: '<S5>/Compare'
|
|
* RelationalOperator: '<S6>/Compare'
|
|
*/
|
|
rtb_Compare_d = ((ADM_Integrated_Logic_U.GV_Vx_Fbk >= 1.0) &&
|
|
(ADM_Integrated_Logic_U.GV_IMU_AX_Val >= 0.5));
|
|
|
|
/* Outputs for Enabled SubSystem: '<S1>/Enabled Subsystem' incorporates:
|
|
* EnablePort: '<S10>/Enable'
|
|
*/
|
|
if (rtb_Compare_d) {
|
|
/* Outport: '<Root>/GV_VCU_EstMass' incorporates:
|
|
* Constant: '<S1>/Constant9'
|
|
* Inport: '<Root>/GV_IMU_AZ_Val'
|
|
* MATLAB Function: '<S10>/MATLAB Function'
|
|
* MATLAB Function: '<S1>/MATLAB Function'
|
|
*/
|
|
ADM_Integrated_Logic_Y.GV_VCU_EstMass = rtb_Switch2 / ((0.01 *
|
|
ADM_Integrated_Logic_U.GV_IMU_AZ_Val + ADM_Integrated_Logic_DW.x_hat[1]) *
|
|
0.022698910310142498);
|
|
}
|
|
|
|
/* End of Outputs for SubSystem: '<S1>/Enabled Subsystem' */
|
|
|
|
/* Outport: '<Root>/GV_VCU_EstMassFlag' */
|
|
ADM_Integrated_Logic_Y.GV_VCU_EstMassFlag = rtb_Compare_d;
|
|
|
|
/* Outport: '<Root>/GV_Motor_Torque_Cmd' */
|
|
ADM_Integrated_Logic_Y.GV_Motor_Torque_Cmd = rtb_Switch2;
|
|
|
|
/* Outport: '<Root>/Target_MCU_Out' incorporates:
|
|
* MATLAB Function: '<S1>/MCU_Fault_Injection'
|
|
*/
|
|
ADM_Integrated_Logic_Y.Target_MCU_Out = rtb_MotorTorque;
|
|
|
|
/* Chart: '<S12>/Chart' incorporates:
|
|
* Constant: '<S12>/Constant'
|
|
* Constant: '<S12>/Constant1'
|
|
* Inport: '<Root>/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: '<Root>/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: '<Root>/Debug_HAC_RPM_Decision' */
|
|
ADM_Integrated_Logic_Y.Debug_HAC_RPM_Decision = 1.0;
|
|
}
|
|
|
|
/* End of Chart: '<S12>/Chart' */
|
|
|
|
/* Saturate: '<S12>/Pitch_Saturation' incorporates:
|
|
* DiscreteIntegrator: '<S12>/Integrator_2'
|
|
*/
|
|
if (ADM_Integrated_Logic_DW.Integrator_2_DSTATE > 8.0) {
|
|
rtb_MotorTorque = 8.0;
|
|
} else if (ADM_Integrated_Logic_DW.Integrator_2_DSTATE < -8.0) {
|
|
rtb_MotorTorque = -8.0;
|
|
} else {
|
|
rtb_MotorTorque = ADM_Integrated_Logic_DW.Integrator_2_DSTATE;
|
|
}
|
|
|
|
/* End of Saturate: '<S12>/Pitch_Saturation' */
|
|
|
|
/* Delay: '<S61>/Delay' */
|
|
if (ADM_Integrated_Logic_DW.icLoad) {
|
|
ADM_Integrated_Logic_DW.Delay_DSTATE_o = rtb_MotorTorque;
|
|
}
|
|
|
|
/* Product: '<S61>/delta rise limit' incorporates:
|
|
* Constant: '<S12>/Angle_Upper'
|
|
* SampleTimeMath: '<S61>/sample time'
|
|
*
|
|
* About '<S61>/sample time':
|
|
* y = K where K = ( w * Ts )
|
|
* */
|
|
rtb_Switch2 = 0.002;
|
|
|
|
/* Sum: '<S61>/Difference Inputs1' incorporates:
|
|
* Delay: '<S61>/Delay'
|
|
*
|
|
* Block description for '<S61>/Difference Inputs1':
|
|
*
|
|
* Add in CPU
|
|
*/
|
|
rtb_MotorTorque -= ADM_Integrated_Logic_DW.Delay_DSTATE_o;
|
|
|
|
/* Switch: '<S66>/Switch2' incorporates:
|
|
* RelationalOperator: '<S66>/LowerRelop1'
|
|
*/
|
|
if (!(rtb_MotorTorque > 0.002)) {
|
|
/* Switch: '<S66>/Switch' incorporates:
|
|
* RelationalOperator: '<S66>/UpperRelop'
|
|
*/
|
|
if (rtb_MotorTorque < -0.002) {
|
|
rtb_Switch2 = -0.002;
|
|
} else {
|
|
rtb_Switch2 = rtb_MotorTorque;
|
|
}
|
|
|
|
/* End of Switch: '<S66>/Switch' */
|
|
}
|
|
|
|
/* End of Switch: '<S66>/Switch2' */
|
|
|
|
/* Sum: '<S61>/Difference Inputs2' incorporates:
|
|
* Delay: '<S61>/Delay'
|
|
*
|
|
* Block description for '<S61>/Difference Inputs2':
|
|
*
|
|
* Add in CPU
|
|
*/
|
|
rtb_MotorTorque = rtb_Switch2 + ADM_Integrated_Logic_DW.Delay_DSTATE_o;
|
|
|
|
/* MATLAB Function: '<S54>/Calculate_F_c' incorporates:
|
|
* Gain: '<S54>/Gain'
|
|
*/
|
|
ADM_Integrated_Lo_Calculate_F_c(ADM_Integrated_Logic_ConstB.W_value,
|
|
0.017453292519943295 * rtb_MotorTorque, &rtb_Switch2);
|
|
|
|
/* Gain: '<S54>/Gain2' incorporates:
|
|
* Constant: '<S54>/Radius'
|
|
* Gain: '<S54>/Gain1'
|
|
* Gain: '<S54>/Rolling_Gain'
|
|
* MATLAB Function: '<S54>/Calculate_F_R'
|
|
* Product: '<S54>/Multiply3'
|
|
* Sum: '<S54>/Required_Brake_Cal'
|
|
*/
|
|
rtb_Pitch_Angle = (cos(0.017453292519943295 * rtb_MotorTorque) *
|
|
436.7465753424658 * 0.0 + rtb_Switch2) * 0.292 *
|
|
0.083822296730930432;
|
|
|
|
/* Saturate: '<S54>/Saturation' */
|
|
if (rtb_Pitch_Angle > 60.0) {
|
|
rtb_Pitch_Angle = 60.0;
|
|
} else if (rtb_Pitch_Angle < 0.0) {
|
|
rtb_Pitch_Angle = 0.0;
|
|
}
|
|
|
|
/* End of Saturate: '<S54>/Saturation' */
|
|
|
|
/* Delay: '<S60>/Delay' */
|
|
if (ADM_Integrated_Logic_DW.icLoad_e) {
|
|
ADM_Integrated_Logic_DW.Delay_DSTATE_i = rtb_Pitch_Angle;
|
|
}
|
|
|
|
/* Product: '<S60>/delta rise limit' incorporates:
|
|
* Constant: '<S12>/Upper_Torq'
|
|
* SampleTimeMath: '<S60>/sample time'
|
|
*
|
|
* About '<S60>/sample time':
|
|
* y = K where K = ( w * Ts )
|
|
* */
|
|
rtb_Switch2 = 0.016;
|
|
|
|
/* Sum: '<S60>/Difference Inputs1' incorporates:
|
|
* Delay: '<S60>/Delay'
|
|
*
|
|
* Block description for '<S60>/Difference Inputs1':
|
|
*
|
|
* Add in CPU
|
|
*/
|
|
rtb_Pitch_Angle -= ADM_Integrated_Logic_DW.Delay_DSTATE_i;
|
|
|
|
/* Switch: '<S65>/Switch2' incorporates:
|
|
* RelationalOperator: '<S65>/LowerRelop1'
|
|
*/
|
|
if (!(rtb_Pitch_Angle > 0.016)) {
|
|
/* Switch: '<S65>/Switch' incorporates:
|
|
* RelationalOperator: '<S65>/UpperRelop'
|
|
*/
|
|
if (rtb_Pitch_Angle < -0.16) {
|
|
rtb_Switch2 = -0.16;
|
|
} else {
|
|
rtb_Switch2 = rtb_Pitch_Angle;
|
|
}
|
|
|
|
/* End of Switch: '<S65>/Switch' */
|
|
}
|
|
|
|
/* End of Switch: '<S65>/Switch2' */
|
|
|
|
/* Sum: '<S60>/Difference Inputs2' incorporates:
|
|
* Delay: '<S60>/Delay'
|
|
*
|
|
* Block description for '<S60>/Difference Inputs2':
|
|
*
|
|
* Add in CPU
|
|
*/
|
|
rtb_Pitch_Angle = rtb_Switch2 + ADM_Integrated_Logic_DW.Delay_DSTATE_i;
|
|
|
|
/* Gain: '<S12>/Grade_GAIN' */
|
|
rtb_Desired_Torque = 0.8 * rtb_Pitch_Angle;
|
|
|
|
/* MATLAB Function: '<S12>/HAC_OFF_OK_Func' */
|
|
rtb_Brake_Torque_Cmd = 0.0;
|
|
|
|
/* Outport: '<Root>/Debug_HAC_FLAG' incorporates:
|
|
* MATLAB Function: '<S12>/HAC_OFF_OK_Func'
|
|
*/
|
|
ADM_Integrated_Logic_Y.Debug_HAC_FLAG = 0.0;
|
|
|
|
/* MATLAB Function: '<S12>/HAC_OFF_OK_Func' incorporates:
|
|
* Constant: '<S12>/Accel_Cmd '
|
|
* Constant: '<S12>/Brake_Cmd'
|
|
* Constant: '<S12>/Gear_D'
|
|
* Inport: '<Root>/GV_BrakeTorqueCommand'
|
|
* Inport: '<Root>/GV_MCU_EstTrq'
|
|
* Outport: '<Root>/Debug_HAC_RPM_Decision'
|
|
* RelationalOperator: '<S12>/Relational Operator'
|
|
* RelationalOperator: '<S12>/Relational Operator1'
|
|
* RelationalOperator: '<S12>/Relational Operator2'
|
|
* Sum: '<S12>/HAC_Flags_Add'
|
|
*/
|
|
if ((double)(((ADM_Integrated_Logic_U.GV_BrakeTorqueCommand <= 20.0) +
|
|
(rtb_Vx_Cmd_R <= 2.0)) + (rtb_RateLimiter == 3.0)) +
|
|
ADM_Integrated_Logic_Y.Debug_HAC_RPM_Decision >= 4.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: '<Root>/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: '<Root>/Debug_HAC_FLAG' */
|
|
ADM_Integrated_Logic_Y.Debug_HAC_FLAG = 1.0;
|
|
}
|
|
}
|
|
|
|
/* Gain: '<S7>/Brake_GAIN' incorporates:
|
|
* DiscreteTransferFcn: '<S1>/Discrete Transfer Fcn'
|
|
*/
|
|
rtb_Product1_c = 0.0625 * ADM_Integrated_Logic_DW.DiscreteTransferFcn_states *
|
|
-80.0;
|
|
|
|
/* MATLAB Function: '<S7>/GearCondition_Brake' */
|
|
if (rtb_RateLimiter == 1.0) {
|
|
idx = -1;
|
|
} else {
|
|
idx = (rtb_RateLimiter == 3.0);
|
|
}
|
|
|
|
/* Saturate: '<S7>/Saturation' */
|
|
if (rtb_Product1_c > 1000.0) {
|
|
rtb_Product1_c = 1000.0;
|
|
} else if (rtb_Product1_c < 0.0) {
|
|
rtb_Product1_c = 0.0;
|
|
}
|
|
|
|
/* Product: '<S7>/Multiply2' incorporates:
|
|
* MATLAB Function: '<S7>/GearCondition_Brake'
|
|
* Saturate: '<S7>/Saturation'
|
|
*/
|
|
rtb_Switch2 = (double)idx * rtb_Product1_c;
|
|
|
|
/* RateLimiter: '<S7>/Brake_Out_RateLimiter' */
|
|
rtb_Product1_c = rtb_Switch2 - ADM_Integrated_Logic_DW.PrevY_o;
|
|
if (rtb_Product1_c > 3.0) {
|
|
rtb_Switch2 = ADM_Integrated_Logic_DW.PrevY_o + 3.0;
|
|
} else if (rtb_Product1_c < -3.0) {
|
|
rtb_Switch2 = ADM_Integrated_Logic_DW.PrevY_o - 3.0;
|
|
}
|
|
|
|
ADM_Integrated_Logic_DW.PrevY_o = rtb_Switch2;
|
|
|
|
/* End of RateLimiter: '<S7>/Brake_Out_RateLimiter' */
|
|
|
|
/* Saturate: '<S7>/Brake_Saturation' */
|
|
if (rtb_Switch2 > 1000.0) {
|
|
rtb_Switch2 = 1000.0;
|
|
} else if (rtb_Switch2 < 0.0) {
|
|
rtb_Switch2 = 0.0;
|
|
}
|
|
|
|
/* End of Saturate: '<S7>/Brake_Saturation' */
|
|
|
|
/* MATLAB Function: '<S1>/Emergency_Brake_Func' incorporates:
|
|
* Inport: '<Root>/GV_VCU_AccControlFlag'
|
|
* Inport: '<Root>/GV_Vx_Fbk'
|
|
* Sum: '<S1>/Add'
|
|
* Switch: '<S1>/Switch2'
|
|
*/
|
|
if (rtb_Compare) {
|
|
rtb_BrakeTorque = ADM_Integrated_Logic_U.GV_Vx_Fbk * 100.0;
|
|
} else {
|
|
if (!(ADM_Integrated_Logic_U.GV_VCU_AccControlFlag > 0.0)) {
|
|
/* Switch: '<S1>/Switch2' */
|
|
rtb_BrakeTorque = rtb_Switch2;
|
|
}
|
|
|
|
rtb_BrakeTorque += rtb_Brake_Torque_Cmd;
|
|
}
|
|
|
|
/* Saturate: '<S1>/Brake_Saturation' incorporates:
|
|
* MATLAB Function: '<S1>/Emergency_Brake_Func'
|
|
*/
|
|
if (rtb_BrakeTorque > 2500.0) {
|
|
rtb_BrakeTorque = 2500.0;
|
|
} else if (rtb_BrakeTorque < 0.0) {
|
|
rtb_BrakeTorque = 0.0;
|
|
}
|
|
|
|
/* End of Saturate: '<S1>/Brake_Saturation' */
|
|
|
|
/* Outport: '<Root>/GV_Brake_Command' incorporates:
|
|
* MATLAB Function: '<S1>/IDB_Fault_Injection'
|
|
*/
|
|
ADM_Integrated_Logic_Y.GV_Brake_Command = rtb_BrakeTorque;
|
|
|
|
/* Outport: '<Root>/Target_IDB_Out' incorporates:
|
|
* MATLAB Function: '<S1>/IDB_Fault_Injection'
|
|
*/
|
|
ADM_Integrated_Logic_Y.Target_IDB_Out = rtb_BrakeTorque;
|
|
|
|
/* Outport: '<Root>/Debug_CC_Brake_Output' */
|
|
ADM_Integrated_Logic_Y.Debug_CC_Brake_Output = rtb_Switch2;
|
|
|
|
/* Outport: '<Root>/Debug_HAC_Brake_Output' */
|
|
ADM_Integrated_Logic_Y.Debug_HAC_Brake_Output = rtb_Brake_Torque_Cmd;
|
|
|
|
/* 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_MotorTorque;
|
|
|
|
/* MATLAB Function: '<S54>/Calculate_F_c1' incorporates:
|
|
* Gain: '<S54>/Gain3'
|
|
*/
|
|
ADM_Integrated_Lo_Calculate_F_c(ADM_Integrated_Logic_ConstB.W_Value_for_Brake,
|
|
0.017453292519943295 * rtb_MotorTorque, &rtb_Switch2);
|
|
|
|
/* Product: '<S55>/Product1' incorporates:
|
|
* Constant: '<S55>/Constant'
|
|
* Gain: '<S55>/gain'
|
|
* Inport: '<Root>/GV_IMU_AX_Val'
|
|
* Product: '<S55>/Product11'
|
|
* Sum: '<S55>/Sum1'
|
|
* Sum: '<S55>/Sum2'
|
|
* Sum: '<S55>/Sum3'
|
|
* Sum: '<S55>/Sum4'
|
|
* UnitDelay: '<S55>/d'
|
|
* UnitDelay: '<S55>/d1'
|
|
*/
|
|
rtb_BrakeTorque = ((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: '<S56>/Product1' incorporates:
|
|
* Constant: '<S56>/Constant'
|
|
* Gain: '<S56>/gain'
|
|
* Inport: '<Root>/GV_IMU_AY_Val'
|
|
* Product: '<S56>/Product11'
|
|
* Sum: '<S56>/Sum1'
|
|
* Sum: '<S56>/Sum2'
|
|
* Sum: '<S56>/Sum3'
|
|
* Sum: '<S56>/Sum4'
|
|
* UnitDelay: '<S56>/d'
|
|
* UnitDelay: '<S56>/d1'
|
|
*/
|
|
rtb_Product1_c = ((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: '<S57>/Product1' incorporates:
|
|
* Constant: '<S57>/Constant'
|
|
* Gain: '<S57>/gain'
|
|
* Inport: '<Root>/GV_IMU_AZ_Val'
|
|
* Product: '<S57>/Product11'
|
|
* Sum: '<S57>/Sum1'
|
|
* Sum: '<S57>/Sum2'
|
|
* Sum: '<S57>/Sum3'
|
|
* Sum: '<S57>/Sum4'
|
|
* UnitDelay: '<S57>/d'
|
|
* UnitDelay: '<S57>/d1'
|
|
*/
|
|
rtb_Desired_Torque = ((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: '<S12>/Pitch_calculate' */
|
|
rtb_Switch2 = sqrt(rtb_Product1_c * rtb_Product1_c + rtb_Desired_Torque *
|
|
rtb_Desired_Torque);
|
|
if (!(rtb_Switch2 == 0.0)) {
|
|
rtb_Switch2 = atan(rtb_BrakeTorque / rtb_Switch2);
|
|
}
|
|
|
|
/* Sum: '<S12>/Sum' incorporates:
|
|
* DiscreteIntegrator: '<S12>/Integrator_2'
|
|
* MATLAB Function: '<S12>/Pitch_calculate'
|
|
*/
|
|
rtb_Brake_Torque_Cmd = ADM_Integrated_Logic_DW.Integrator_2_DSTATE -
|
|
rtb_Switch2 * 57.295779513082323;
|
|
|
|
/* Product: '<S58>/Product1' incorporates:
|
|
* Constant: '<S58>/Constant'
|
|
* Gain: '<S58>/gain'
|
|
* Inport: '<Root>/GV_IMU_PitchRtVal'
|
|
* Product: '<S58>/Product11'
|
|
* Sum: '<S58>/Sum1'
|
|
* Sum: '<S58>/Sum2'
|
|
* Sum: '<S58>/Sum3'
|
|
* Sum: '<S58>/Sum4'
|
|
* UnitDelay: '<S58>/d'
|
|
* UnitDelay: '<S58>/d1'
|
|
*/
|
|
rtb_Product1_lo = ((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>/Add2' incorporates:
|
|
* Inport: '<Root>/GV_Vx_Fbk'
|
|
*/
|
|
rtb_Vx_Cmd_R -= ADM_Integrated_Logic_U.GV_Vx_Fbk;
|
|
|
|
/* Outport: '<Root>/GV_Gear_Postion_Out' */
|
|
ADM_Integrated_Logic_Y.GV_Gear_Postion_Out = rtb_RateLimiter;
|
|
|
|
/* 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_c);
|
|
|
|
/* Product: '<S22>/delta rise limit' incorporates:
|
|
* SampleTimeMath: '<S22>/sample time'
|
|
*
|
|
* About '<S22>/sample time':
|
|
* y = K where K = ( w * Ts )
|
|
* */
|
|
rtb_Switch2 = rtb_y_c * 0.002;
|
|
|
|
/* Delay: '<S22>/Delay' incorporates:
|
|
* Inport: '<Root>/GV_RWA_RackAngleCommand'
|
|
*/
|
|
if (ADM_Integrated_Logic_DW.icLoad_l) {
|
|
ADM_Integrated_Logic_DW.Delay_DSTATE_c =
|
|
ADM_Integrated_Logic_U.GV_RWA_RackAngleCommand;
|
|
}
|
|
|
|
/* Sum: '<S22>/Difference Inputs1' incorporates:
|
|
* Delay: '<S22>/Delay'
|
|
* Inport: '<Root>/GV_RWA_RackAngleCommand'
|
|
*
|
|
* Block description for '<S22>/Difference Inputs1':
|
|
*
|
|
* Add in CPU
|
|
*/
|
|
rtb_UkYk1_j = ADM_Integrated_Logic_U.GV_RWA_RackAngleCommand -
|
|
ADM_Integrated_Logic_DW.Delay_DSTATE_c;
|
|
|
|
/* MATLAB Function: '<S1>/MATLAB Function2' incorporates:
|
|
* Gain: '<S1>/Gain3'
|
|
* Sum: '<S1>/Add1'
|
|
*/
|
|
ADM_Integrated__MATLABFunction2(-rtb_Add1_tmp, &rtb_y_c);
|
|
|
|
/* Switch: '<S67>/Switch2' incorporates:
|
|
* RelationalOperator: '<S67>/LowerRelop1'
|
|
*/
|
|
if (!(rtb_UkYk1_j > rtb_Switch2)) {
|
|
/* Product: '<S22>/delta fall limit' incorporates:
|
|
* SampleTimeMath: '<S22>/sample time'
|
|
*
|
|
* About '<S22>/sample time':
|
|
* y = K where K = ( w * Ts )
|
|
* */
|
|
rtb_Switch2 = 0.002 * rtb_y_c;
|
|
|
|
/* Switch: '<S67>/Switch' incorporates:
|
|
* RelationalOperator: '<S67>/UpperRelop'
|
|
*/
|
|
if (!(rtb_UkYk1_j < rtb_Switch2)) {
|
|
rtb_Switch2 = rtb_UkYk1_j;
|
|
}
|
|
|
|
/* End of Switch: '<S67>/Switch' */
|
|
}
|
|
|
|
/* End of Switch: '<S67>/Switch2' */
|
|
|
|
/* Sum: '<S22>/Difference Inputs2' incorporates:
|
|
* Delay: '<S22>/Delay'
|
|
*
|
|
* Block description for '<S22>/Difference Inputs2':
|
|
*
|
|
* Add in CPU
|
|
*/
|
|
rtb_UkYk1_j = rtb_Switch2 + ADM_Integrated_Logic_DW.Delay_DSTATE_c;
|
|
|
|
/* Outport: '<Root>/GV_Master_Rack_Angle_Cmd' incorporates:
|
|
* MATLAB Function: '<S1>/RWA_Actuator_Fault_Injection'
|
|
*/
|
|
ADM_Integrated_Logic_Y.GV_Master_Rack_Angle_Cmd = rtb_UkYk1_j;
|
|
|
|
/* Outport: '<Root>/Target_RWA_Out' incorporates:
|
|
* MATLAB Function: '<S1>/RWA_Actuator_Fault_Injection'
|
|
*/
|
|
ADM_Integrated_Logic_Y.Target_RWA_Out = rtb_UkYk1_j;
|
|
|
|
/* MATLAB Function: '<S1>/MATLAB Function5' */
|
|
ADM_Integrated__MATLABFunction1(rtb_Add1_tmp, &rtb_y_c);
|
|
|
|
/* Product: '<S23>/delta rise limit' incorporates:
|
|
* SampleTimeMath: '<S23>/sample time'
|
|
*
|
|
* About '<S23>/sample time':
|
|
* y = K where K = ( w * Ts )
|
|
* */
|
|
rtb_Switch2 = rtb_y_c * 0.002;
|
|
|
|
/* Delay: '<S23>/Delay' incorporates:
|
|
* Inport: '<Root>/GV_RWS_RackAngleCommand'
|
|
*/
|
|
if (ADM_Integrated_Logic_DW.icLoad_d) {
|
|
ADM_Integrated_Logic_DW.Delay_DSTATE_i1 =
|
|
ADM_Integrated_Logic_U.GV_RWS_RackAngleCommand;
|
|
}
|
|
|
|
/* Sum: '<S23>/Difference Inputs1' incorporates:
|
|
* Delay: '<S23>/Delay'
|
|
* Inport: '<Root>/GV_RWS_RackAngleCommand'
|
|
*
|
|
* Block description for '<S23>/Difference Inputs1':
|
|
*
|
|
* Add in CPU
|
|
*/
|
|
rtb_Add1 = ADM_Integrated_Logic_U.GV_RWS_RackAngleCommand -
|
|
ADM_Integrated_Logic_DW.Delay_DSTATE_i1;
|
|
|
|
/* MATLAB Function: '<S1>/MATLAB Function6' incorporates:
|
|
* Gain: '<S1>/Gain5'
|
|
*/
|
|
ADM_Integrated__MATLABFunction2(-rtb_Add1_tmp, &rtb_y_c);
|
|
|
|
/* Switch: '<S68>/Switch2' incorporates:
|
|
* RelationalOperator: '<S68>/LowerRelop1'
|
|
*/
|
|
if (!(rtb_Add1 > rtb_Switch2)) {
|
|
/* Product: '<S23>/delta fall limit' incorporates:
|
|
* SampleTimeMath: '<S23>/sample time'
|
|
*
|
|
* About '<S23>/sample time':
|
|
* y = K where K = ( w * Ts )
|
|
* */
|
|
rtb_Switch2 = 0.002 * rtb_y_c;
|
|
|
|
/* Switch: '<S68>/Switch' incorporates:
|
|
* RelationalOperator: '<S68>/UpperRelop'
|
|
*/
|
|
if (!(rtb_Add1 < rtb_Switch2)) {
|
|
rtb_Switch2 = rtb_Add1;
|
|
}
|
|
|
|
/* End of Switch: '<S68>/Switch' */
|
|
}
|
|
|
|
/* End of Switch: '<S68>/Switch2' */
|
|
|
|
/* Sum: '<S23>/Difference Inputs2' incorporates:
|
|
* Delay: '<S23>/Delay'
|
|
*
|
|
* Block description for '<S23>/Difference Inputs2':
|
|
*
|
|
* Add in CPU
|
|
*/
|
|
rtb_Switch2 += ADM_Integrated_Logic_DW.Delay_DSTATE_i1;
|
|
|
|
/* Outport: '<Root>/GV_RWS_RackAngleCmd1' */
|
|
ADM_Integrated_Logic_Y.GV_RWS_RackAngleCmd1 = rtb_Switch2;
|
|
|
|
/* Outport: '<Root>/Act_Fault_Exist' incorporates:
|
|
* MATLAB Function: '<S1>/Actuator_Fault_Decision'
|
|
*/
|
|
ADM_Integrated_Logic_Y.Act_Fault_Exist = 0.0;
|
|
|
|
/* MATLAB Function: '<S1>/Actuator_Fault_Decision' incorporates:
|
|
* Inport: '<Root>/GV_ACU_ECU_Fault_Flag'
|
|
* Inport: '<Root>/GV_ACU_ECU_Fault_Flag1'
|
|
* Inport: '<Root>/GV_ACU_ECU_Fault_Flag2'
|
|
* Inport: '<Root>/GV_ACU_ECU_Fault_Flag3'
|
|
* Inport: '<Root>/GV_ACU_Fault_Flag'
|
|
* Inport: '<Root>/GV_Drive_Mode'
|
|
* Inport: '<Root>/GV_IDB_ECU_Fault_Flag'
|
|
* Inport: '<Root>/GV_MCU_Actuator_Fault_Flag'
|
|
* Inport: '<Root>/GV_RCU_ECU_Fault_Flag'
|
|
* Inport: '<Root>/GV_RWA1_ECU_Fault_Flag'
|
|
* Inport: '<Root>/GV_RWA2_ECU_Fault_Flag'
|
|
* Inport: '<Root>/GV_RWA_Actuator_Fault'
|
|
*/
|
|
if ((ADM_Integrated_Logic_U.GV_ACU_Fault_Flag == 1.0) ||
|
|
(ADM_Integrated_Logic_U.GV_MCU_Actuator_Fault_Flag == 1.0) ||
|
|
(ADM_Integrated_Logic_U.GV_RWA_Actuator_Fault == 1.0)) {
|
|
/* Outport: '<Root>/Act_Fault_Exist' */
|
|
ADM_Integrated_Logic_Y.Act_Fault_Exist = 1.0;
|
|
}
|
|
|
|
if (((ADM_Integrated_Logic_U.GV_ACU_ECU_Fault_Flag == 1.0) ||
|
|
(ADM_Integrated_Logic_U.GV_ACU_ECU_Fault_Flag1 == 1.0) ||
|
|
(ADM_Integrated_Logic_U.GV_ACU_ECU_Fault_Flag2 == 1.0) ||
|
|
(ADM_Integrated_Logic_U.GV_ACU_ECU_Fault_Flag3 == 1.0)) &&
|
|
((ADM_Integrated_Logic_U.GV_Drive_Mode == 0.0) ||
|
|
(ADM_Integrated_Logic_U.GV_Drive_Mode == 2.0))) {
|
|
/* Outport: '<Root>/Act_Fault_Exist' */
|
|
ADM_Integrated_Logic_Y.Act_Fault_Exist = 1.0;
|
|
}
|
|
|
|
if ((ADM_Integrated_Logic_U.GV_IDB_ECU_Fault_Flag == 1.0) ||
|
|
(ADM_Integrated_Logic_U.GV_RCU_ECU_Fault_Flag == 1.0)) {
|
|
/* Outport: '<Root>/Act_Fault_Exist' */
|
|
ADM_Integrated_Logic_Y.Act_Fault_Exist = 1.0;
|
|
} else if ((ADM_Integrated_Logic_U.GV_RWA1_ECU_Fault_Flag == 1.0) &&
|
|
(ADM_Integrated_Logic_U.GV_RWA2_ECU_Fault_Flag == 1.0)) {
|
|
/* Outport: '<Root>/Act_Fault_Exist' */
|
|
ADM_Integrated_Logic_Y.Act_Fault_Exist = 1.0;
|
|
}
|
|
|
|
/* Update for UnitDelay: '<S14>/d1' incorporates:
|
|
* Inport: '<Root>/GV_IMU_AX_Val'
|
|
*/
|
|
ADM_Integrated_Logic_DW.d1_DSTATE = ADM_Integrated_Logic_U.GV_IMU_AX_Val;
|
|
|
|
/* Update for UnitDelay: '<S14>/d' */
|
|
ADM_Integrated_Logic_DW.d_DSTATE = rtb_Gain;
|
|
|
|
/* Update for DiscreteIntegrator: '<S25>/Discrete-Time Integrator' */
|
|
ADM_Integrated_Logic_DW.DiscreteTimeIntegrator_DSTATE += 0.002 * rtb_Gain2_j;
|
|
|
|
/* Update for UnitDelay: '<S28>/UD'
|
|
*
|
|
* Block description for '<S28>/UD':
|
|
*
|
|
* Store in Global RAM
|
|
*/
|
|
ADM_Integrated_Logic_DW.UD_DSTATE = rtb_TSamp;
|
|
|
|
/* Update for Delay: '<S11>/Memory' */
|
|
ADM_Integrated_Logic_DW.Memory_DSTATE = rtb_RateLimiter;
|
|
|
|
/* Update for UnitDelay: '<S34>/d1' */
|
|
ADM_Integrated_Logic_DW.d1_DSTATE_c = rtb_Target_RPM;
|
|
|
|
/* Update for UnitDelay: '<S34>/d' */
|
|
ADM_Integrated_Logic_DW.d_DSTATE_l = rtb_Product1_j;
|
|
|
|
/* Update for UnitDelay: '<S44>/d' */
|
|
ADM_Integrated_Logic_DW.d_DSTATE_i = rtb_Product1_j;
|
|
|
|
/* Update for UnitDelay: '<S44>/d1' */
|
|
ADM_Integrated_Logic_DW.d1_DSTATE_p = rtb_Sum1_i2;
|
|
|
|
/* Update for UnitDelay: '<S45>/d' */
|
|
ADM_Integrated_Logic_DW.d_DSTATE_c = rtb_Sum1_i2;
|
|
|
|
/* Update for UnitDelay: '<S45>/d1' */
|
|
ADM_Integrated_Logic_DW.d1_DSTATE_h = rtb_Sum1_o1;
|
|
|
|
/* Update for UnitDelay: '<S46>/d' */
|
|
ADM_Integrated_Logic_DW.d_DSTATE_d = rtb_Sum1_o1;
|
|
|
|
/* Update for UnitDelay: '<S46>/d1' */
|
|
ADM_Integrated_Logic_DW.d1_DSTATE_l = rtb_Sum1_lm;
|
|
|
|
/* Update for Delay: '<S48>/Delay1' */
|
|
ADM_Integrated_Logic_DW.Delay1_DSTATE = rtb_Gain_c;
|
|
|
|
/* Update for Delay: '<S48>/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: '<S48>/Delay2' */
|
|
ADM_Integrated_Logic_DW.Delay2_DSTATE = rtb_Product1_dc;
|
|
|
|
/* Update for Delay: '<S48>/Delay3' */
|
|
ADM_Integrated_Logic_DW.Delay3_DSTATE[0] =
|
|
ADM_Integrated_Logic_DW.Delay3_DSTATE[1];
|
|
ADM_Integrated_Logic_DW.Delay3_DSTATE[1] = rtb_Product1_dc;
|
|
|
|
/* Update for UnitDelay: '<S47>/d1' */
|
|
ADM_Integrated_Logic_DW.d1_DSTATE_e = rtb_Product1_dc;
|
|
|
|
/* Update for UnitDelay: '<S47>/d' */
|
|
ADM_Integrated_Logic_DW.d_DSTATE_ij = rtb_Product1_h;
|
|
|
|
/* Update for Memory: '<S7>/Memory' */
|
|
ADM_Integrated_Logic_DW.Memory_PreviousInput = rtb_Add;
|
|
|
|
/* Update for UnitDelay: '<S36>/d' */
|
|
ADM_Integrated_Logic_DW.d_DSTATE_ir = rtb_Product1_j;
|
|
|
|
/* Update for UnitDelay: '<S36>/d1' */
|
|
ADM_Integrated_Logic_DW.d1_DSTATE_o = rtb_Error_m;
|
|
|
|
/* Update for UnitDelay: '<S37>/d' */
|
|
ADM_Integrated_Logic_DW.d_DSTATE_m = rtb_Error_m;
|
|
|
|
/* Update for UnitDelay: '<S37>/d1' */
|
|
ADM_Integrated_Logic_DW.d1_DSTATE_hm = rtb_Sum1_aj;
|
|
|
|
/* Update for UnitDelay: '<S38>/d' */
|
|
ADM_Integrated_Logic_DW.d_DSTATE_mw = rtb_Sum1_aj;
|
|
|
|
/* Update for UnitDelay: '<S38>/d1' */
|
|
ADM_Integrated_Logic_DW.d1_DSTATE_g = rtb_Sum1_p;
|
|
|
|
/* Update for UnitDelay: '<S39>/d1' */
|
|
ADM_Integrated_Logic_DW.d1_DSTATE_ej = rtb_Add3;
|
|
|
|
/* Update for UnitDelay: '<S39>/d' */
|
|
ADM_Integrated_Logic_DW.d_DSTATE_j = rtb_Product1_nu;
|
|
|
|
/* Update for Delay: '<S40>/Delay1' */
|
|
ADM_Integrated_Logic_DW.Delay1_DSTATE_c = rtb_Product1_nu;
|
|
|
|
/* Update for Delay: '<S40>/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_nu;
|
|
|
|
/* Update for Delay: '<S40>/Delay2' */
|
|
ADM_Integrated_Logic_DW.Delay2_DSTATE_n = rtb_Product1_p;
|
|
|
|
/* Update for Delay: '<S40>/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_p;
|
|
|
|
/* Update for DiscreteIntegrator: '<S12>/Integrator_2' incorporates:
|
|
* Constant: '<S12>/I_gain'
|
|
* Constant: '<S12>/P_gain'
|
|
* DiscreteIntegrator: '<S12>/Integrator_1'
|
|
* Product: '<S12>/Product'
|
|
* Product: '<S12>/Product1'
|
|
* Sum: '<S12>/Sum1'
|
|
* Sum: '<S12>/Sum2'
|
|
*/
|
|
ADM_Integrated_Logic_DW.Integrator_2_DSTATE += (rtb_Product1_lo -
|
|
(ADM_Integrated_Logic_DW.Integrator_1_DSTATE * 5.0 + rtb_Brake_Torque_Cmd *
|
|
100.0)) * 0.002;
|
|
|
|
/* Update for Delay: '<S61>/Delay' */
|
|
ADM_Integrated_Logic_DW.icLoad = false;
|
|
ADM_Integrated_Logic_DW.Delay_DSTATE_o = rtb_MotorTorque;
|
|
|
|
/* Update for Delay: '<S60>/Delay' */
|
|
ADM_Integrated_Logic_DW.icLoad_e = false;
|
|
ADM_Integrated_Logic_DW.Delay_DSTATE_i = rtb_Pitch_Angle;
|
|
|
|
/* 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: '<S55>/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: '<S55>/d' */
|
|
ADM_Integrated_Logic_DW.d_DSTATE_e = rtb_BrakeTorque;
|
|
|
|
/* Update for UnitDelay: '<S56>/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: '<S56>/d' */
|
|
ADM_Integrated_Logic_DW.d_DSTATE_p = rtb_Product1_c;
|
|
|
|
/* Update for UnitDelay: '<S57>/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: '<S57>/d' */
|
|
ADM_Integrated_Logic_DW.d_DSTATE_n = rtb_Desired_Torque;
|
|
|
|
/* Update for DiscreteIntegrator: '<S12>/Integrator_1' */
|
|
ADM_Integrated_Logic_DW.Integrator_1_DSTATE += 0.002 * rtb_Brake_Torque_Cmd;
|
|
|
|
/* Update for UnitDelay: '<S58>/d1' incorporates:
|
|
* Inport: '<Root>/GV_IMU_PitchRtVal'
|
|
*/
|
|
ADM_Integrated_Logic_DW.d1_DSTATE_a = ADM_Integrated_Logic_U.GV_IMU_PitchRtVal;
|
|
|
|
/* Update for UnitDelay: '<S58>/d' */
|
|
ADM_Integrated_Logic_DW.d_DSTATE_d1 = rtb_Product1_lo;
|
|
|
|
/* Update for Delay: '<S22>/Delay' */
|
|
ADM_Integrated_Logic_DW.icLoad_l = false;
|
|
ADM_Integrated_Logic_DW.Delay_DSTATE_c = rtb_UkYk1_j;
|
|
|
|
/* Update for Delay: '<S23>/Delay' */
|
|
ADM_Integrated_Logic_DW.icLoad_d = false;
|
|
ADM_Integrated_Logic_DW.Delay_DSTATE_i1 = rtb_Switch2;
|
|
}
|
|
|
|
/* Model initialize function */
|
|
void ADM_Integrated_Logic_initialize(void)
|
|
{
|
|
/* Registration code */
|
|
|
|
/* initialize non-finites */
|
|
rt_InitInfAndNaN(sizeof(double));
|
|
|
|
/* InitializeConditions for Delay: '<S61>/Delay' */
|
|
ADM_Integrated_Logic_DW.icLoad = true;
|
|
|
|
/* InitializeConditions for Delay: '<S60>/Delay' */
|
|
ADM_Integrated_Logic_DW.icLoad_e = true;
|
|
|
|
/* InitializeConditions for Delay: '<S22>/Delay' */
|
|
ADM_Integrated_Logic_DW.icLoad_l = true;
|
|
|
|
/* InitializeConditions for Delay: '<S23>/Delay' */
|
|
ADM_Integrated_Logic_DW.icLoad_d = true;
|
|
}
|
|
|
|
/*
|
|
* File trailer for generated code.
|
|
*
|
|
* [EOF]
|
|
*/
|