/* Outputs for root system: '<Root>' */
static void mdlOutputs(SimStruct *S, int_T tid)
{
((BlockIO *) _ssGetBlockIO(S))->B_0_1_0 = ((real_T)((D_Work *) ssGetRootDWork
(S))->clockTickCounter < ((Parameters *) ssGetDefaultParam(S))->P_6) &&
(((D_Work *) ssGetRootDWork(S))->clockTickCounter >= 0) ? ((Parameters *)
ssGetDefaultParam(S))->P_4 : 0.0;
if ((real_T)((D_Work *) ssGetRootDWork(S))->clockTickCounter >= ((Parameters *)
ssGetDefaultParam(S))->P_5 - 1.0) {
((D_Work *) ssGetRootDWork(S))->clockTickCounter = 0;
} else {
((D_Work *) ssGetRootDWork(S))->clockTickCounter = ((D_Work *)
ssGetRootDWork(S))->clockTickCounter + 1;
}
((BlockIO *) _ssGetBlockIO(S))->B_0_2_0 = (muDoubleScalarSin(((Parameters *)
ssGetDefaultParam(S))->P_2 * ssGetTaskTime(S,0) + ((Parameters *)
ssGetDefaultParam(S))->P_3) * ((Parameters *) ssGetDefaultParam(S))->P_0 +
((Parameters *) ssGetDefaultParam(S))->P_1) * ((BlockIO *) _ssGetBlockIO(S)
)->B_0_1_0;
/* Scope: '<Root>/Scope' */
/* Call into Simulink for Scope */
ssCallAccelRunBlock(S, 0, 3, SS_CALL_MDL_OUTPUTS);
/* Scope: '<Root>/Scope2' */
/* Call into Simulink for Scope */
ssCallAccelRunBlock(S, 0, 4, SS_CALL_MDL_OUTPUTS);
/* tid is required for a uniform function interface.
* Argument tid is not used in the function. */
UNUSED_PARAMETER(tid);
}
localDW ) { localDW -> nej2n2fr4t = false ; } void bemu4fk2sq ( SimStruct *
const S , boolean_T fuz4opuchm , real_T nkqzvcukuv , bkp1n1bm33 * localB ,
mdhkgmfpbe * localDW , mvw1gbh0c1 * localP , pfdaqg4dlv * localXdis ) { if (
ssIsSampleHit ( S , 1 , 0 ) && ssIsMajorTimeStep ( S ) ) { if ( fuz4opuchm )
{ if ( ! localDW -> nej2n2fr4t ) { if ( ssGetTaskTime ( S , 1 ) !=
ssGetTStart ( S ) ) { ssSetSolverNeedsReset ( S ) ; } ( void ) memset ( & (
localXdis -> fq23fwubtb ) , 0 , 2 * sizeof ( boolean_T ) ) ; localDW ->
nej2n2fr4t = true ; } } else { if ( localDW -> nej2n2fr4t ) {
ssSetSolverNeedsReset ( S ) ; ( void ) memset ( & ( localXdis -> fq23fwubtb )
, 1 , 2 * sizeof ( boolean_T ) ) ; mctqnytrkq ( localDW ) ; } } } if (
localDW -> nej2n2fr4t ) { localB -> obmjfhqv1x = ( muDoubleScalarSin ( localP
-> P_4 * ssGetTaskTime ( S , 0 ) + localP -> P_5 ) * localP -> P_2 + localP
-> P_3 ) * nkqzvcukuv ; localB -> n024o4o5og = ( muDoubleScalarSin ( localP
-> P_8 * ssGetTaskTime ( S , 0 ) + localP -> P_9 ) * localP -> P_6 + localP
-> P_7 ) * nkqzvcukuv ; if ( ssIsMajorTimeStep ( S ) ) { srUpdateBC ( localDW
-> mdukihclqs ) ; } } } void jreinmfd0u ( bkp1n1bm33 * localB , mdhkgmfpbe *
/* Function Definitions */
void b_exp(creal_T x_data[1280], int32_T x_size[1])
{
int32_T i32;
int32_T k;
real_T r;
real_T x_data_im;
real_T b_x_data_im;
i32 = x_size[0];
for (k = 0; k < i32; k++) {
r = muDoubleScalarExp(x_data[k].re / 2.0);
x_data_im = x_data[k].im;
b_x_data_im = x_data[k].im;
x_data[k].re = r * (r * muDoubleScalarCos(x_data_im));
x_data[k].im = r * (r * muDoubleScalarSin(b_x_data_im));
}
}
static void derivatives_c1_newtons_cradle(SFc1_newtons_cradleInstanceStruct
*chartInstance)
{
int32_T c1_i1;
real_T (*c1_p_dot)[3];
real_T (*c1_v_dot)[3];
real_T (*c1_v)[3];
real_T (*c1_p)[3];
c1_v_dot = (real_T (*)[3])(ssGetdX(chartInstance->S) + 3);
c1_v = (real_T (*)[3])(ssGetContStates(chartInstance->S) + 3);
c1_p_dot = (real_T (*)[3])(ssGetdX(chartInstance->S) + 0);
c1_p = (real_T (*)[3])(ssGetContStates(chartInstance->S) + 0);
for (c1_i1 = 0; c1_i1 < 3; c1_i1++) {
(*c1_p_dot)[c1_i1] = 0.0;
(*c1_v_dot)[c1_i1] = 0.0;
}
_sfTime_ = (real_T)ssGetT(chartInstance->S);
for (c1_i1 = 0; c1_i1 < 3; c1_i1++) {
(*c1_p_dot)[c1_i1] = (*c1_v)[c1_i1];
(*c1_v_dot)[c1_i1] = muDoubleScalarSin((*c1_p)[c1_i1]);
(*c1_v_dot)[c1_i1] *= -9.81;
}
}
static void sf_c16_Final_Project_v3b(SFc16_Final_Project_v3bInstanceStruct
*chartInstance)
{
int32_T c16_i4;
int32_T c16_i5;
real_T c16_hoistedGlobal;
real_T c16_b_hoistedGlobal;
real_T c16_c_hoistedGlobal;
real_T c16_d_hoistedGlobal;
real_T c16_e_hoistedGlobal;
real_T c16_f_hoistedGlobal;
real_T c16_u1;
real_T c16_u2;
real_T c16_u3;
real_T c16_u4;
real_T c16_u5;
real_T c16_u6;
uint32_T c16_debug_family_var_map[11];
real_T c16_theta;
real_T c16_nargin = 6.0;
real_T c16_nargout = 2.0;
real_T c16_y[9];
real_T c16_R[4];
real_T c16_x;
real_T c16_b_x;
real_T c16_c_x;
real_T c16_d_x;
real_T c16_e_x;
real_T c16_f_x;
real_T c16_g_x;
real_T c16_h_x;
int32_T c16_i6;
int32_T c16_i7;
real_T *c16_b_u1;
real_T *c16_b_u2;
real_T *c16_b_u3;
real_T *c16_b_u4;
real_T *c16_b_u5;
real_T *c16_b_u6;
real_T (*c16_b_R)[4];
real_T (*c16_b_y)[9];
c16_b_u6 = (real_T *)ssGetInputPortSignal(chartInstance->S, 5);
c16_b_u5 = (real_T *)ssGetInputPortSignal(chartInstance->S, 4);
c16_b_u4 = (real_T *)ssGetInputPortSignal(chartInstance->S, 3);
c16_b_u3 = (real_T *)ssGetInputPortSignal(chartInstance->S, 2);
c16_b_u2 = (real_T *)ssGetInputPortSignal(chartInstance->S, 1);
c16_b_R = (real_T (*)[4])ssGetOutputPortSignal(chartInstance->S, 2);
c16_b_y = (real_T (*)[9])ssGetOutputPortSignal(chartInstance->S, 1);
c16_b_u1 = (real_T *)ssGetInputPortSignal(chartInstance->S, 0);
_sfTime_ = (real_T)ssGetT(chartInstance->S);
_SFD_CC_CALL(CHART_ENTER_SFUNCTION_TAG, 15U, chartInstance->c16_sfEvent);
_SFD_DATA_RANGE_CHECK(*c16_b_u1, 0U);
for (c16_i4 = 0; c16_i4 < 9; c16_i4++) {
_SFD_DATA_RANGE_CHECK((*c16_b_y)[c16_i4], 1U);
}
for (c16_i5 = 0; c16_i5 < 4; c16_i5++) {
_SFD_DATA_RANGE_CHECK((*c16_b_R)[c16_i5], 2U);
}
_SFD_DATA_RANGE_CHECK(*c16_b_u2, 3U);
_SFD_DATA_RANGE_CHECK(*c16_b_u3, 4U);
_SFD_DATA_RANGE_CHECK(*c16_b_u4, 5U);
_SFD_DATA_RANGE_CHECK(*c16_b_u5, 6U);
_SFD_DATA_RANGE_CHECK(*c16_b_u6, 7U);
chartInstance->c16_sfEvent = CALL_EVENT;
_SFD_CC_CALL(CHART_ENTER_DURING_FUNCTION_TAG, 15U, chartInstance->c16_sfEvent);
c16_hoistedGlobal = *c16_b_u1;
c16_b_hoistedGlobal = *c16_b_u2;
c16_c_hoistedGlobal = *c16_b_u3;
c16_d_hoistedGlobal = *c16_b_u4;
c16_e_hoistedGlobal = *c16_b_u5;
c16_f_hoistedGlobal = *c16_b_u6;
c16_u1 = c16_hoistedGlobal;
c16_u2 = c16_b_hoistedGlobal;
c16_u3 = c16_c_hoistedGlobal;
c16_u4 = c16_d_hoistedGlobal;
c16_u5 = c16_e_hoistedGlobal;
c16_u6 = c16_f_hoistedGlobal;
_SFD_SYMBOL_SCOPE_PUSH_EML(0U, 11U, 11U, c16_debug_family_names,
c16_debug_family_var_map);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c16_theta, 0U, c16_c_sf_marshallOut,
c16_c_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c16_nargin, 1U, c16_c_sf_marshallOut,
c16_c_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c16_nargout, 2U, c16_c_sf_marshallOut,
c16_c_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML(&c16_u1, 3U, c16_c_sf_marshallOut);
_SFD_SYMBOL_SCOPE_ADD_EML(&c16_u2, 4U, c16_c_sf_marshallOut);
_SFD_SYMBOL_SCOPE_ADD_EML(&c16_u3, 5U, c16_c_sf_marshallOut);
_SFD_SYMBOL_SCOPE_ADD_EML(&c16_u4, 6U, c16_c_sf_marshallOut);
_SFD_SYMBOL_SCOPE_ADD_EML(&c16_u5, 7U, c16_c_sf_marshallOut);
_SFD_SYMBOL_SCOPE_ADD_EML(&c16_u6, 8U, c16_c_sf_marshallOut);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(c16_y, 9U, c16_b_sf_marshallOut,
c16_b_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(c16_R, 10U, c16_sf_marshallOut,
c16_sf_marshallIn);
CV_EML_FCN(0, 0);
_SFD_EML_CALL(0U, chartInstance->c16_sfEvent, 6);
c16_y[0] = c16_u1;
c16_y[1] = c16_u2;
c16_y[2] = 0.0;
c16_y[3] = c16_u4;
c16_y[4] = c16_u5;
c16_y[5] = 0.0;
c16_y[6] = 0.0;
c16_y[7] = 0.0;
c16_y[8] = c16_u6;
_SFD_EML_CALL(0U, chartInstance->c16_sfEvent, 7);
c16_theta = c16_u3;
_SFD_EML_CALL(0U, chartInstance->c16_sfEvent, 9);
c16_x = c16_theta;
c16_b_x = c16_x;
c16_b_x = muDoubleScalarCos(c16_b_x);
c16_c_x = c16_theta;
c16_d_x = c16_c_x;
c16_d_x = muDoubleScalarSin(c16_d_x);
c16_e_x = c16_theta;
c16_f_x = c16_e_x;
c16_f_x = muDoubleScalarSin(c16_f_x);
c16_g_x = c16_theta;
c16_h_x = c16_g_x;
c16_h_x = muDoubleScalarCos(c16_h_x);
c16_R[0] = c16_b_x;
c16_R[2] = -c16_d_x;
c16_R[1] = c16_f_x;
c16_R[3] = c16_h_x;
_SFD_EML_CALL(0U, chartInstance->c16_sfEvent, -9);
_SFD_SYMBOL_SCOPE_POP();
for (c16_i6 = 0; c16_i6 < 9; c16_i6++) {
(*c16_b_y)[c16_i6] = c16_y[c16_i6];
}
for (c16_i7 = 0; c16_i7 < 4; c16_i7++) {
(*c16_b_R)[c16_i7] = c16_R[c16_i7];
}
_SFD_CC_CALL(EXIT_OUT_OF_FUNCTION_TAG, 15U, chartInstance->c16_sfEvent);
_SFD_CHECK_FOR_STATE_INCONSISTENCY(_Final_Project_v3bMachineNumber_,
chartInstance->chartNumber, chartInstance->instanceNumber);
}
static void c2_chartstep_c2_Quadsim(SFc2_QuadsimInstanceStruct *chartInstance)
{
int32_T c2_i4;
real_T c2_Xin[16];
uint32_T c2_debug_family_var_map[40];
real_T c2_arm_l;
real_T c2_craft_m;
real_T c2_Ixx;
real_T c2_Iyy;
real_T c2_Izz;
real_T c2_b;
real_T c2_d;
real_T c2_g;
real_T c2_X_dot;
real_T c2_Y_dot;
real_T c2_Z_dot;
real_T c2_Roll_dot;
real_T c2_Pitch_dot;
real_T c2_Yaw_dot;
real_T c2_X;
real_T c2_Y;
real_T c2_Z;
real_T c2_Roll;
real_T c2_Pitch;
real_T c2_Yaw;
real_T c2_F;
real_T c2_R;
real_T c2_B;
real_T c2_L;
real_T c2_TF;
real_T c2_TR;
real_T c2_TB;
real_T c2_TL;
real_T c2_D;
real_T c2_X_ddot;
real_T c2_Y_ddot;
real_T c2_z_craft_component;
real_T c2_Z_ddot;
real_T c2_Roll_ddot;
real_T c2_Pitch_ddot;
real_T c2_Yaw_ddot;
real_T c2_nargin = 1.0;
real_T c2_nargout = 1.0;
real_T c2_Xout[12];
real_T c2_b_b;
real_T c2_c_b;
real_T c2_d_b;
real_T c2_e_b;
real_T c2_f_b;
real_T c2_x;
real_T c2_b_x;
real_T c2_c_x;
real_T c2_d_x;
real_T c2_a;
real_T c2_g_b;
real_T c2_y;
real_T c2_e_x;
real_T c2_f_x;
real_T c2_b_a;
real_T c2_h_b;
real_T c2_b_y;
real_T c2_g_x;
real_T c2_h_x;
real_T c2_i_x;
real_T c2_j_x;
real_T c2_c_a;
real_T c2_i_b;
real_T c2_c_y;
real_T c2_j_b;
real_T c2_d_y;
real_T c2_d_a;
real_T c2_k_b;
real_T c2_k_x;
real_T c2_l_x;
real_T c2_m_x;
real_T c2_n_x;
real_T c2_e_a;
real_T c2_l_b;
real_T c2_e_y;
real_T c2_o_x;
real_T c2_p_x;
real_T c2_f_a;
real_T c2_m_b;
real_T c2_f_y;
real_T c2_q_x;
real_T c2_r_x;
real_T c2_s_x;
real_T c2_t_x;
real_T c2_g_a;
real_T c2_n_b;
real_T c2_g_y;
real_T c2_o_b;
real_T c2_h_y;
real_T c2_h_a;
real_T c2_p_b;
real_T c2_u_x;
real_T c2_v_x;
real_T c2_q_b;
real_T c2_i_y;
real_T c2_w_x;
real_T c2_x_x;
real_T c2_i_a;
real_T c2_r_b;
real_T c2_j_y;
real_T c2_j_a;
real_T c2_s_b;
real_T c2_t_b;
real_T c2_u_b;
real_T c2_A;
real_T c2_y_x;
real_T c2_ab_x;
int32_T c2_i5;
real_T (*c2_b_Xout)[12];
real_T (*c2_b_Xin)[16];
c2_b_Xin = (real_T (*)[16])ssGetInputPortSignal(chartInstance->S, 0);
c2_b_Xout = (real_T (*)[12])ssGetOutputPortSignal(chartInstance->S, 1);
_SFD_CC_CALL(CHART_ENTER_DURING_FUNCTION_TAG, 1U, chartInstance->c2_sfEvent);
for (c2_i4 = 0; c2_i4 < 16; c2_i4++) {
c2_Xin[c2_i4] = (*c2_b_Xin)[c2_i4];
}
sf_debug_symbol_scope_push_eml(0U, 40U, 40U, c2_debug_family_names,
c2_debug_family_var_map);
sf_debug_symbol_scope_add_eml(&c2_arm_l, 0U, c2_c_sf_marshallOut);
sf_debug_symbol_scope_add_eml(&c2_craft_m, 1U, c2_c_sf_marshallOut);
sf_debug_symbol_scope_add_eml(&c2_Ixx, 2U, c2_c_sf_marshallOut);
sf_debug_symbol_scope_add_eml(&c2_Iyy, 3U, c2_c_sf_marshallOut);
sf_debug_symbol_scope_add_eml(&c2_Izz, 4U, c2_c_sf_marshallOut);
sf_debug_symbol_scope_add_eml(&c2_b, 5U, c2_c_sf_marshallOut);
sf_debug_symbol_scope_add_eml(&c2_d, 6U, c2_c_sf_marshallOut);
sf_debug_symbol_scope_add_eml_importable(&c2_g, 7U, c2_c_sf_marshallOut,
c2_b_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c2_X_dot, 8U, c2_c_sf_marshallOut,
c2_b_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c2_Y_dot, 9U, c2_c_sf_marshallOut,
c2_b_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c2_Z_dot, 10U, c2_c_sf_marshallOut,
c2_b_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c2_Roll_dot, 11U,
c2_c_sf_marshallOut, c2_b_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c2_Pitch_dot, 12U,
c2_c_sf_marshallOut, c2_b_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c2_Yaw_dot, 13U, c2_c_sf_marshallOut,
c2_b_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c2_X, 14U, c2_c_sf_marshallOut,
c2_b_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c2_Y, 15U, c2_c_sf_marshallOut,
c2_b_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c2_Z, 16U, c2_c_sf_marshallOut,
c2_b_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c2_Roll, 17U, c2_c_sf_marshallOut,
c2_b_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c2_Pitch, 18U, c2_c_sf_marshallOut,
c2_b_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c2_Yaw, 19U, c2_c_sf_marshallOut,
c2_b_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c2_F, 20U, c2_c_sf_marshallOut,
c2_b_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c2_R, 21U, c2_c_sf_marshallOut,
c2_b_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c2_B, 22U, c2_c_sf_marshallOut,
c2_b_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c2_L, 23U, c2_c_sf_marshallOut,
c2_b_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c2_TF, 24U, c2_c_sf_marshallOut,
c2_b_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c2_TR, 25U, c2_c_sf_marshallOut,
c2_b_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c2_TB, 26U, c2_c_sf_marshallOut,
c2_b_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c2_TL, 27U, c2_c_sf_marshallOut,
c2_b_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c2_D, 28U, c2_c_sf_marshallOut,
c2_b_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c2_X_ddot, 29U, c2_c_sf_marshallOut,
c2_b_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c2_Y_ddot, 30U, c2_c_sf_marshallOut,
c2_b_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c2_z_craft_component, 31U,
c2_c_sf_marshallOut, c2_b_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c2_Z_ddot, 32U, c2_c_sf_marshallOut,
c2_b_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c2_Roll_ddot, 33U,
c2_c_sf_marshallOut, c2_b_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c2_Pitch_ddot, 34U,
c2_c_sf_marshallOut, c2_b_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c2_Yaw_ddot, 35U,
c2_c_sf_marshallOut, c2_b_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c2_nargin, 36U, c2_c_sf_marshallOut,
c2_b_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c2_nargout, 37U, c2_c_sf_marshallOut,
c2_b_sf_marshallIn);
sf_debug_symbol_scope_add_eml(c2_Xin, 38U, c2_b_sf_marshallOut);
sf_debug_symbol_scope_add_eml_importable(c2_Xout, 39U, c2_sf_marshallOut,
c2_sf_marshallIn);
CV_EML_FCN(0, 0);
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, 3);
c2_arm_l = 0.23;
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, 4);
c2_craft_m = 0.95;
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, 5);
c2_Ixx = 0.0075;
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, 6);
c2_Iyy = 0.0075;
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, 7);
c2_Izz = 0.013000000000000001;
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, 8);
c2_b = 3.1299999999999995E-5;
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, 9);
c2_d = 7.5E-7;
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, 10);
c2_g = 9.81;
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, 13);
c2_X_dot = c2_Xin[0];
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, 14);
c2_Y_dot = c2_Xin[1];
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, 15);
c2_Z_dot = c2_Xin[2];
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, 16);
c2_Roll_dot = c2_Xin[3];
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, 17);
c2_Pitch_dot = c2_Xin[4];
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, 18);
c2_Yaw_dot = c2_Xin[5];
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, 19);
c2_X = c2_Xin[6];
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, 20);
c2_Y = c2_Xin[7];
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, 21);
c2_Z = c2_Xin[8];
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, 22);
c2_Roll = c2_Xin[9];
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, 23);
c2_Pitch = c2_Xin[10];
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, 24);
c2_Yaw = c2_Xin[11];
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, 33);
c2_F = c2_Xin[12];
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, 34);
c2_R = c2_Xin[13];
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, 35);
c2_B = c2_Xin[14];
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, 36);
c2_L = c2_Xin[15];
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, 39);
c2_b_b = c2_mpower(chartInstance, c2_F);
c2_TF = 3.1299999999999995E-5 * c2_b_b;
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, 40);
c2_c_b = c2_mpower(chartInstance, c2_R);
c2_TR = 3.1299999999999995E-5 * c2_c_b;
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, 41);
c2_d_b = c2_mpower(chartInstance, c2_B);
c2_TB = 3.1299999999999995E-5 * c2_d_b;
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, 42);
c2_e_b = c2_mpower(chartInstance, c2_L);
c2_TL = 3.1299999999999995E-5 * c2_e_b;
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, 43);
c2_f_b = ((-c2_F + c2_R) - c2_B) + c2_L;
c2_D = 7.5E-7 * c2_f_b;
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, 56);
c2_x = c2_Roll;
c2_b_x = c2_x;
c2_b_x = muDoubleScalarCos(c2_b_x);
c2_c_x = c2_Pitch;
c2_d_x = c2_c_x;
c2_d_x = muDoubleScalarSin(c2_d_x);
c2_a = c2_b_x;
c2_g_b = c2_d_x;
c2_y = c2_a * c2_g_b;
c2_e_x = c2_Yaw;
c2_f_x = c2_e_x;
c2_f_x = muDoubleScalarCos(c2_f_x);
c2_b_a = c2_y;
c2_h_b = c2_f_x;
c2_b_y = c2_b_a * c2_h_b;
c2_g_x = c2_Roll;
c2_h_x = c2_g_x;
c2_h_x = muDoubleScalarSin(c2_h_x);
c2_i_x = c2_Yaw;
c2_j_x = c2_i_x;
c2_j_x = muDoubleScalarSin(c2_j_x);
c2_c_a = c2_h_x;
c2_i_b = c2_j_x;
c2_c_y = c2_c_a * c2_i_b;
c2_j_b = c2_b_y + c2_c_y;
c2_d_y = 1.0526315789473684 * c2_j_b;
c2_d_a = c2_d_y;
c2_k_b = ((c2_TF + c2_TR) + c2_TB) + c2_TL;
c2_X_ddot = c2_d_a * c2_k_b;
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, 57);
c2_k_x = c2_Roll;
c2_l_x = c2_k_x;
c2_l_x = muDoubleScalarCos(c2_l_x);
c2_m_x = c2_Pitch;
c2_n_x = c2_m_x;
c2_n_x = muDoubleScalarSin(c2_n_x);
c2_e_a = c2_l_x;
c2_l_b = c2_n_x;
c2_e_y = c2_e_a * c2_l_b;
c2_o_x = c2_Yaw;
c2_p_x = c2_o_x;
c2_p_x = muDoubleScalarSin(c2_p_x);
c2_f_a = c2_e_y;
c2_m_b = c2_p_x;
c2_f_y = c2_f_a * c2_m_b;
c2_q_x = c2_Roll;
c2_r_x = c2_q_x;
c2_r_x = muDoubleScalarSin(c2_r_x);
c2_s_x = c2_Yaw;
c2_t_x = c2_s_x;
c2_t_x = muDoubleScalarCos(c2_t_x);
c2_g_a = c2_r_x;
c2_n_b = c2_t_x;
c2_g_y = c2_g_a * c2_n_b;
c2_o_b = c2_f_y + c2_g_y;
c2_h_y = 1.0526315789473684 * c2_o_b;
c2_h_a = c2_h_y;
c2_p_b = ((c2_TF + c2_TR) + c2_TB) + c2_TL;
c2_Y_ddot = c2_h_a * c2_p_b;
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, 58);
c2_u_x = c2_Roll;
c2_v_x = c2_u_x;
c2_v_x = muDoubleScalarCos(c2_v_x);
c2_q_b = c2_v_x;
c2_i_y = 1.0526315789473684 * c2_q_b;
c2_w_x = c2_Pitch;
c2_x_x = c2_w_x;
c2_x_x = muDoubleScalarCos(c2_x_x);
c2_i_a = c2_i_y;
c2_r_b = c2_x_x;
c2_j_y = c2_i_a * c2_r_b;
c2_j_a = c2_j_y;
c2_s_b = ((c2_TF + c2_TR) + c2_TB) + c2_TL;
c2_z_craft_component = c2_j_a * c2_s_b;
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, 59);
c2_Z_ddot = c2_z_craft_component - c2_g;
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, 60);
c2_t_b = -c2_TR + c2_TL;
c2_Roll_ddot = 30.666666666666668 * c2_t_b;
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, 61);
c2_u_b = c2_TF - c2_TB;
c2_Pitch_ddot = 30.666666666666668 * c2_u_b;
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, 62);
c2_A = c2_D;
c2_y_x = c2_A;
c2_ab_x = c2_y_x;
c2_Yaw_ddot = c2_ab_x / 0.013000000000000001;
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, 65);
c2_Xout[0] = c2_X_ddot;
c2_Xout[1] = c2_Y_ddot;
c2_Xout[2] = c2_Z_ddot;
c2_Xout[3] = c2_Roll_ddot;
c2_Xout[4] = c2_Pitch_ddot;
c2_Xout[5] = c2_Yaw_ddot;
c2_Xout[6] = c2_X_dot;
c2_Xout[7] = c2_Y_dot;
c2_Xout[8] = c2_Z_dot;
c2_Xout[9] = c2_Roll_dot;
c2_Xout[10] = c2_Pitch_dot;
c2_Xout[11] = c2_Yaw_dot;
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, -65);
sf_debug_symbol_scope_pop();
for (c2_i5 = 0; c2_i5 < 12; c2_i5++) {
(*c2_b_Xout)[c2_i5] = c2_Xout[c2_i5];
}
_SFD_CC_CALL(EXIT_OUT_OF_FUNCTION_TAG, 1U, chartInstance->c2_sfEvent);
}
static void sf_c3_V2A1_2(SFc3_V2A1_2InstanceStruct *chartInstance)
{
int32_T c3_i2;
real_T c3_hoistedGlobal;
real_T c3_b_hoistedGlobal;
real_T c3_delta;
real_T c3_hoehe;
uint32_T c3_debug_family_var_map[9];
real_T c3_er;
real_T c3_bogen_delta;
real_T c3_x;
real_T c3_y;
real_T c3_nargin = 2.0;
real_T c3_nargout = 1.0;
real_T c3_x0[2];
real_T c3_a;
real_T c3_b_y;
real_T c3_A;
real_T c3_b_x;
real_T c3_c_x;
real_T c3_d_x;
real_T c3_e_x;
real_T c3_b_a;
real_T c3_b;
real_T c3_f_x;
real_T c3_g_x;
real_T c3_c_a;
real_T c3_b_b;
int32_T c3_i3;
real_T c3_u[2];
const mxArray *c3_c_y = NULL;
int32_T c3_i4;
real_T *c3_b_hoehe;
real_T *c3_b_delta;
real_T (*c3_b_x0)[2];
c3_b_hoehe = (real_T *)ssGetInputPortSignal(chartInstance->S, 1);
c3_b_x0 = (real_T (*)[2])ssGetOutputPortSignal(chartInstance->S, 1);
c3_b_delta = (real_T *)ssGetInputPortSignal(chartInstance->S, 0);
_SFD_SYMBOL_SCOPE_PUSH(0U, 0U);
_sfTime_ = (real_T)ssGetT(chartInstance->S);
_SFD_CC_CALL(CHART_ENTER_SFUNCTION_TAG, 2U, chartInstance->c3_sfEvent);
_SFD_DATA_RANGE_CHECK(*c3_b_delta, 0U);
for (c3_i2 = 0; c3_i2 < 2; c3_i2++) {
_SFD_DATA_RANGE_CHECK((*c3_b_x0)[c3_i2], 1U);
}
_SFD_DATA_RANGE_CHECK(*c3_b_hoehe, 2U);
chartInstance->c3_sfEvent = CALL_EVENT;
_SFD_CC_CALL(CHART_ENTER_DURING_FUNCTION_TAG, 2U, chartInstance->c3_sfEvent);
c3_hoistedGlobal = *c3_b_delta;
c3_b_hoistedGlobal = *c3_b_hoehe;
c3_delta = c3_hoistedGlobal;
c3_hoehe = c3_b_hoistedGlobal;
_SFD_SYMBOL_SCOPE_PUSH_EML(0U, 9U, 9U, c3_debug_family_names,
c3_debug_family_var_map);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c3_er, 0U, c3_b_sf_marshallOut,
c3_b_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c3_bogen_delta, 1U, c3_b_sf_marshallOut,
c3_b_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c3_x, 2U, c3_b_sf_marshallOut,
c3_b_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c3_y, 3U, c3_b_sf_marshallOut,
c3_b_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c3_nargin, 4U, c3_b_sf_marshallOut,
c3_b_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c3_nargout, 5U, c3_b_sf_marshallOut,
c3_b_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML(&c3_delta, 6U, c3_b_sf_marshallOut);
_SFD_SYMBOL_SCOPE_ADD_EML(&c3_hoehe, 7U, c3_b_sf_marshallOut);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(c3_x0, 8U, c3_sf_marshallOut,
c3_sf_marshallIn);
CV_EML_FCN(0, 0);
_SFD_EML_CALL(0U, chartInstance->c3_sfEvent, 2);
c3_er = 6.378E+6;
_SFD_EML_CALL(0U, chartInstance->c3_sfEvent, 3);
c3_a = c3_delta;
c3_b_y = c3_a * 3.1415926535897931;
c3_A = c3_b_y;
c3_b_x = c3_A;
c3_c_x = c3_b_x;
c3_bogen_delta = c3_c_x / 180.0;
_SFD_EML_CALL(0U, chartInstance->c3_sfEvent, 4);
c3_d_x = c3_bogen_delta;
c3_e_x = c3_d_x;
c3_e_x = muDoubleScalarCos(c3_e_x);
c3_b_a = c3_e_x;
c3_b = c3_er + c3_hoehe;
c3_x = c3_b_a * c3_b;
_SFD_EML_CALL(0U, chartInstance->c3_sfEvent, 5);
c3_f_x = c3_bogen_delta;
c3_g_x = c3_f_x;
c3_g_x = muDoubleScalarSin(c3_g_x);
c3_c_a = c3_g_x;
c3_b_b = c3_er + c3_hoehe;
c3_y = c3_c_a * c3_b_b;
_SFD_EML_CALL(0U, chartInstance->c3_sfEvent, 7);
c3_x0[0] = c3_x;
c3_x0[1] = c3_y;
sf_mex_printf("%s =\\n", "x0");
for (c3_i3 = 0; c3_i3 < 2; c3_i3++) {
c3_u[c3_i3] = c3_x0[c3_i3];
}
c3_c_y = NULL;
sf_mex_assign(&c3_c_y, sf_mex_create("y", c3_u, 0, 0U, 1U, 0U, 1, 2), FALSE);
sf_mex_call_debug("disp", 0U, 1U, 14, c3_c_y);
_SFD_EML_CALL(0U, chartInstance->c3_sfEvent, -7);
_SFD_SYMBOL_SCOPE_POP();
for (c3_i4 = 0; c3_i4 < 2; c3_i4++) {
(*c3_b_x0)[c3_i4] = c3_x0[c3_i4];
}
_SFD_CC_CALL(EXIT_OUT_OF_FUNCTION_TAG, 2U, chartInstance->c3_sfEvent);
_SFD_SYMBOL_SCOPE_POP();
_SFD_CHECK_FOR_STATE_INCONSISTENCY(_V2A1_2MachineNumber_,
chartInstance->chartNumber, chartInstance->instanceNumber);
}
static void sf_gateway_c7_aircraft_simplex(SFc7_aircraft_simplexInstanceStruct
*chartInstance)
{
real_T c7_hoistedGlobal;
real_T c7_b_hoistedGlobal;
real_T c7_c_hoistedGlobal;
real_T c7_b_phi;
real_T c7_b_theta;
real_T c7_b_psi;
uint32_T c7_debug_family_var_map[6];
real_T c7_nargin = 3.0;
real_T c7_nargout = 1.0;
real_T c7_b_R[9];
real_T c7_x;
real_T c7_b_x;
real_T c7_c_x;
real_T c7_d_x;
real_T c7_e_x;
real_T c7_f_x;
real_T c7_g_x;
real_T c7_h_x;
real_T c7_i_x;
real_T c7_j_x;
real_T c7_k_x;
real_T c7_l_x;
real_T c7_m_x;
real_T c7_n_x;
real_T c7_o_x;
real_T c7_p_x;
real_T c7_q_x;
real_T c7_r_x;
real_T c7_s_x;
real_T c7_t_x;
real_T c7_u_x;
real_T c7_v_x;
real_T c7_w_x;
real_T c7_x_x;
real_T c7_y_x;
real_T c7_ab_x;
real_T c7_bb_x;
real_T c7_cb_x;
real_T c7_db_x;
real_T c7_eb_x;
real_T c7_fb_x;
real_T c7_gb_x;
real_T c7_hb_x;
real_T c7_ib_x;
real_T c7_jb_x;
real_T c7_kb_x;
real_T c7_lb_x;
real_T c7_mb_x;
real_T c7_nb_x;
real_T c7_ob_x;
real_T c7_pb_x;
real_T c7_qb_x;
real_T c7_rb_x;
real_T c7_sb_x;
real_T c7_tb_x;
real_T c7_ub_x;
real_T c7_vb_x;
real_T c7_wb_x;
real_T c7_xb_x;
real_T c7_yb_x;
real_T c7_ac_x;
real_T c7_bc_x;
real_T c7_cc_x;
real_T c7_dc_x;
real_T c7_ec_x;
real_T c7_fc_x;
real_T c7_gc_x;
real_T c7_hc_x;
int32_T c7_i2;
int32_T c7_i3;
_SFD_SYMBOL_SCOPE_PUSH(0U, 0U);
_sfTime_ = sf_get_time(chartInstance->S);
_SFD_CC_CALL(CHART_ENTER_SFUNCTION_TAG, 0U, chartInstance->c7_sfEvent);
_SFD_DATA_RANGE_CHECK(*chartInstance->c7_phi, 0U);
chartInstance->c7_sfEvent = CALL_EVENT;
_SFD_CC_CALL(CHART_ENTER_DURING_FUNCTION_TAG, 0U, chartInstance->c7_sfEvent);
c7_hoistedGlobal = *chartInstance->c7_phi;
c7_b_hoistedGlobal = *chartInstance->c7_theta;
c7_c_hoistedGlobal = *chartInstance->c7_psi;
c7_b_phi = c7_hoistedGlobal;
c7_b_theta = c7_b_hoistedGlobal;
c7_b_psi = c7_c_hoistedGlobal;
_SFD_SYMBOL_SCOPE_PUSH_EML(0U, 6U, 6U, c7_debug_family_names,
c7_debug_family_var_map);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c7_nargin, 0U, c7_b_sf_marshallOut,
c7_b_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c7_nargout, 1U, c7_b_sf_marshallOut,
c7_b_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML(&c7_b_phi, 2U, c7_b_sf_marshallOut);
_SFD_SYMBOL_SCOPE_ADD_EML(&c7_b_theta, 3U, c7_b_sf_marshallOut);
_SFD_SYMBOL_SCOPE_ADD_EML(&c7_b_psi, 4U, c7_b_sf_marshallOut);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(c7_b_R, 5U, c7_sf_marshallOut,
c7_sf_marshallIn);
CV_EML_FCN(0, 0);
_SFD_EML_CALL(0U, chartInstance->c7_sfEvent, 5);
c7_x = c7_b_theta;
c7_b_x = c7_x;
c7_b_x = muDoubleScalarCos(c7_b_x);
c7_c_x = c7_b_psi;
c7_d_x = c7_c_x;
c7_d_x = muDoubleScalarCos(c7_d_x);
c7_e_x = c7_b_phi;
c7_f_x = c7_e_x;
c7_f_x = muDoubleScalarCos(c7_f_x);
c7_g_x = c7_b_psi;
c7_h_x = c7_g_x;
c7_h_x = muDoubleScalarSin(c7_h_x);
c7_i_x = c7_b_phi;
c7_j_x = c7_i_x;
c7_j_x = muDoubleScalarSin(c7_j_x);
c7_k_x = c7_b_theta;
c7_l_x = c7_k_x;
c7_l_x = muDoubleScalarSin(c7_l_x);
c7_m_x = c7_b_psi;
c7_n_x = c7_m_x;
c7_n_x = muDoubleScalarCos(c7_n_x);
c7_o_x = c7_b_phi;
c7_p_x = c7_o_x;
c7_p_x = muDoubleScalarSin(c7_p_x);
c7_q_x = c7_b_psi;
c7_r_x = c7_q_x;
c7_r_x = muDoubleScalarSin(c7_r_x);
c7_s_x = c7_b_phi;
c7_t_x = c7_s_x;
c7_t_x = muDoubleScalarCos(c7_t_x);
c7_u_x = c7_b_theta;
c7_v_x = c7_u_x;
c7_v_x = muDoubleScalarSin(c7_v_x);
c7_w_x = c7_b_psi;
c7_x_x = c7_w_x;
c7_x_x = muDoubleScalarCos(c7_x_x);
c7_y_x = c7_b_theta;
c7_ab_x = c7_y_x;
c7_ab_x = muDoubleScalarCos(c7_ab_x);
c7_bb_x = c7_b_psi;
c7_cb_x = c7_bb_x;
c7_cb_x = muDoubleScalarSin(c7_cb_x);
c7_db_x = c7_b_phi;
c7_eb_x = c7_db_x;
c7_eb_x = muDoubleScalarCos(c7_eb_x);
c7_fb_x = c7_b_psi;
c7_gb_x = c7_fb_x;
c7_gb_x = muDoubleScalarCos(c7_gb_x);
c7_hb_x = c7_b_phi;
c7_ib_x = c7_hb_x;
c7_ib_x = muDoubleScalarSin(c7_ib_x);
c7_jb_x = c7_b_theta;
c7_kb_x = c7_jb_x;
c7_kb_x = muDoubleScalarSin(c7_kb_x);
c7_lb_x = c7_b_psi;
c7_mb_x = c7_lb_x;
c7_mb_x = muDoubleScalarSin(c7_mb_x);
c7_nb_x = c7_b_phi;
c7_ob_x = c7_nb_x;
c7_ob_x = muDoubleScalarSin(c7_ob_x);
c7_pb_x = c7_b_psi;
c7_qb_x = c7_pb_x;
c7_qb_x = muDoubleScalarCos(c7_qb_x);
c7_rb_x = c7_b_phi;
c7_sb_x = c7_rb_x;
c7_sb_x = muDoubleScalarCos(c7_sb_x);
c7_tb_x = c7_b_theta;
c7_ub_x = c7_tb_x;
c7_ub_x = muDoubleScalarSin(c7_ub_x);
c7_vb_x = c7_b_psi;
c7_wb_x = c7_vb_x;
c7_wb_x = muDoubleScalarSin(c7_wb_x);
c7_xb_x = c7_b_theta;
c7_yb_x = c7_xb_x;
c7_yb_x = muDoubleScalarSin(c7_yb_x);
c7_ac_x = c7_b_phi;
c7_bc_x = c7_ac_x;
c7_bc_x = muDoubleScalarSin(c7_bc_x);
c7_cc_x = c7_b_theta;
c7_dc_x = c7_cc_x;
c7_dc_x = muDoubleScalarCos(c7_dc_x);
c7_ec_x = c7_b_phi;
c7_fc_x = c7_ec_x;
c7_fc_x = muDoubleScalarCos(c7_fc_x);
c7_gc_x = c7_b_theta;
c7_hc_x = c7_gc_x;
c7_hc_x = muDoubleScalarCos(c7_hc_x);
c7_b_R[0] = c7_b_x * c7_d_x;
c7_b_R[3] = c7_f_x * c7_h_x + c7_j_x * c7_l_x * c7_n_x;
c7_b_R[6] = c7_p_x * c7_r_x - c7_t_x * c7_v_x * c7_x_x;
c7_b_R[1] = -c7_ab_x * c7_cb_x;
c7_b_R[4] = c7_eb_x * c7_gb_x - c7_ib_x * c7_kb_x * c7_mb_x;
c7_b_R[7] = c7_ob_x * c7_qb_x + c7_sb_x * c7_ub_x * c7_wb_x;
c7_b_R[2] = c7_yb_x;
c7_b_R[5] = -c7_bc_x * c7_dc_x;
c7_b_R[8] = c7_fc_x * c7_hc_x;
_SFD_EML_CALL(0U, chartInstance->c7_sfEvent, -5);
_SFD_SYMBOL_SCOPE_POP();
for (c7_i2 = 0; c7_i2 < 9; c7_i2++) {
(*chartInstance->c7_R)[c7_i2] = c7_b_R[c7_i2];
}
_SFD_CC_CALL(EXIT_OUT_OF_FUNCTION_TAG, 0U, chartInstance->c7_sfEvent);
_SFD_SYMBOL_SCOPE_POP();
_SFD_CHECK_FOR_STATE_INCONSISTENCY(_aircraft_simplexMachineNumber_,
chartInstance->chartNumber, chartInstance->instanceNumber);
for (c7_i3 = 0; c7_i3 < 9; c7_i3++) {
_SFD_DATA_RANGE_CHECK((*chartInstance->c7_R)[c7_i3], 1U);
}
_SFD_DATA_RANGE_CHECK(*chartInstance->c7_theta, 2U);
_SFD_DATA_RANGE_CHECK(*chartInstance->c7_psi, 3U);
}
static void sf_c27_Model_justmodel(SFc27_Model_justmodelInstanceStruct
*chartInstance)
{
int32_T c27_i0;
real_T c27_hoistedGlobal;
real_T c27_b_hoistedGlobal;
real_T c27_U;
int32_T c27_i1;
real_T c27_eta_ship[3];
real_T c27_alpha_LOS;
uint32_T c27_debug_family_var_map[9];
real_T c27_psi;
real_T c27_gamma;
real_T c27_nargin = 3.0;
real_T c27_nargout = 2.0;
real_T c27_surge;
real_T c27_sway;
real_T c27_x;
real_T c27_b_x;
real_T c27_a;
real_T c27_b;
real_T c27_c_x;
real_T c27_d_x;
real_T c27_b_a;
real_T c27_b_b;
real_T *c27_b_U;
real_T *c27_b_surge;
real_T *c27_b_alpha_LOS;
real_T *c27_b_sway;
real_T (*c27_b_eta_ship)[3];
c27_b_sway = (real_T *)ssGetOutputPortSignal(chartInstance->S, 2);
c27_b_alpha_LOS = (real_T *)ssGetInputPortSignal(chartInstance->S, 2);
c27_b_eta_ship = (real_T (*)[3])ssGetInputPortSignal(chartInstance->S, 1);
c27_b_surge = (real_T *)ssGetOutputPortSignal(chartInstance->S, 1);
c27_b_U = (real_T *)ssGetInputPortSignal(chartInstance->S, 0);
_sfTime_ = (real_T)ssGetT(chartInstance->S);
_SFD_CC_CALL(CHART_ENTER_SFUNCTION_TAG, 17U, chartInstance->c27_sfEvent);
_SFD_DATA_RANGE_CHECK(*c27_b_U, 0U);
_SFD_DATA_RANGE_CHECK(*c27_b_surge, 1U);
for (c27_i0 = 0; c27_i0 < 3; c27_i0++) {
_SFD_DATA_RANGE_CHECK((*c27_b_eta_ship)[c27_i0], 2U);
}
_SFD_DATA_RANGE_CHECK(*c27_b_alpha_LOS, 3U);
_SFD_DATA_RANGE_CHECK(*c27_b_sway, 4U);
chartInstance->c27_sfEvent = CALL_EVENT;
_SFD_CC_CALL(CHART_ENTER_DURING_FUNCTION_TAG, 17U, chartInstance->c27_sfEvent);
c27_hoistedGlobal = *c27_b_U;
c27_b_hoistedGlobal = *c27_b_alpha_LOS;
c27_U = c27_hoistedGlobal;
for (c27_i1 = 0; c27_i1 < 3; c27_i1++) {
c27_eta_ship[c27_i1] = (*c27_b_eta_ship)[c27_i1];
}
c27_alpha_LOS = c27_b_hoistedGlobal;
_SFD_SYMBOL_SCOPE_PUSH_EML(0U, 9U, 9U, c27_debug_family_names,
c27_debug_family_var_map);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c27_psi, 0U, c27_sf_marshallOut,
c27_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c27_gamma, 1U, c27_sf_marshallOut,
c27_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c27_nargin, 2U, c27_sf_marshallOut,
c27_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c27_nargout, 3U, c27_sf_marshallOut,
c27_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML(&c27_U, 4U, c27_sf_marshallOut);
_SFD_SYMBOL_SCOPE_ADD_EML(c27_eta_ship, 5U, c27_b_sf_marshallOut);
_SFD_SYMBOL_SCOPE_ADD_EML(&c27_alpha_LOS, 6U, c27_sf_marshallOut);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c27_surge, 7U, c27_sf_marshallOut,
c27_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c27_sway, 8U, c27_sf_marshallOut,
c27_sf_marshallIn);
CV_EML_FCN(0, 0);
_SFD_EML_CALL(0U, chartInstance->c27_sfEvent, 3);
c27_psi = c27_eta_ship[2];
_SFD_EML_CALL(0U, chartInstance->c27_sfEvent, 4);
c27_gamma = c27_alpha_LOS - c27_psi;
_SFD_EML_CALL(0U, chartInstance->c27_sfEvent, 6);
c27_x = c27_gamma;
c27_b_x = c27_x;
c27_b_x = muDoubleScalarCos(c27_b_x);
c27_a = c27_U;
c27_b = c27_b_x;
c27_surge = c27_a * c27_b;
_SFD_EML_CALL(0U, chartInstance->c27_sfEvent, 7);
c27_c_x = c27_gamma;
c27_d_x = c27_c_x;
c27_d_x = muDoubleScalarSin(c27_d_x);
c27_b_a = c27_U;
c27_b_b = c27_d_x;
c27_sway = c27_b_a * c27_b_b;
_SFD_EML_CALL(0U, chartInstance->c27_sfEvent, -7);
_SFD_SYMBOL_SCOPE_POP();
*c27_b_surge = c27_surge;
*c27_b_sway = c27_sway;
_SFD_CC_CALL(EXIT_OUT_OF_FUNCTION_TAG, 17U, chartInstance->c27_sfEvent);
_SFD_CHECK_FOR_STATE_INCONSISTENCY(_Model_justmodelMachineNumber_,
chartInstance->chartNumber, chartInstance->instanceNumber);
}
static void c30_c30_adcs_v15_integral_Power(void)
{
int32_T c30_i5;
real_T c30_r_I[3];
real_T c30_t;
real_T c30_nargout = 1.0;
real_T c30_nargin = 4.0;
real_T c30_c_equinox = c30_b_equinox;
real_T c30_c_today = c30_b_today;
real_T c30_TEI[9];
real_T c30_phi;
real_T c30_st_sec;
real_T c30_ut_sec;
real_T c30_W_EARTH_ROT;
real_T c30_stperut;
real_T c30_r_ECEF[3];
real_T c30_b;
real_T c30_a;
real_T c30_x;
real_T c30_b_x;
real_T c30_c_x;
real_T c30_d_x;
real_T c30_e_x;
real_T c30_f_x;
real_T c30_g_x;
real_T c30_h_x;
real_T c30_i_x;
real_T c30_j_x;
real_T c30_k_x;
real_T c30_l_x;
int32_T c30_i6;
int32_T c30_i7;
static real_T c30_dv1[3] = { 0.0, 0.0, 1.0 };
int32_T c30_i8;
real_T c30_b_a[9];
int32_T c30_i9;
real_T c30_b_b[3];
int32_T c30_i10;
real_T c30_A[9];
int32_T c30_i11;
real_T c30_B[3];
int32_T c30_i12;
int32_T c30_i13;
real_T c30_b_A[9];
int32_T c30_i14;
real_T c30_b_B[3];
int32_T c30_i15;
real_T c30_c_A[9];
int32_T c30_i16;
real_T c30_c_B[3];
int32_T c30_i17;
int32_T c30_i18;
int32_T c30_i19;
int32_T c30_i20;
real_T *c30_b_t;
real_T (*c30_b_r_ECEF)[3];
real_T (*c30_b_r_I)[3];
c30_b_r_ECEF = (real_T (*)[3])ssGetOutputPortSignal(chartInstance.S, 1);
c30_b_t = (real_T *)ssGetInputPortSignal(chartInstance.S, 1);
c30_b_r_I = (real_T (*)[3])ssGetInputPortSignal(chartInstance.S, 0);
_SFD_CC_CALL(CHART_ENTER_DURING_FUNCTION_TAG,23);
for (c30_i5 = 0; c30_i5 < 3; c30_i5 = c30_i5 + 1) {
c30_r_I[c30_i5] = (*c30_b_r_I)[c30_i5];
}
c30_t = *c30_b_t;
sf_debug_symbol_scope_push(13U, 0U);
sf_debug_symbol_scope_add("nargout", &c30_nargout, c30_b_sf_marshall);
sf_debug_symbol_scope_add("nargin", &c30_nargin, c30_b_sf_marshall);
sf_debug_symbol_scope_add("equinox", &c30_c_equinox, c30_b_sf_marshall);
sf_debug_symbol_scope_add("today", &c30_c_today, c30_b_sf_marshall);
sf_debug_symbol_scope_add("TEI", &c30_TEI, c30_c_sf_marshall);
sf_debug_symbol_scope_add("phi", &c30_phi, c30_b_sf_marshall);
sf_debug_symbol_scope_add("st_sec", &c30_st_sec, c30_b_sf_marshall);
sf_debug_symbol_scope_add("ut_sec", &c30_ut_sec, c30_b_sf_marshall);
sf_debug_symbol_scope_add("W_EARTH_ROT", &c30_W_EARTH_ROT, c30_b_sf_marshall);
sf_debug_symbol_scope_add("stperut", &c30_stperut, c30_b_sf_marshall);
sf_debug_symbol_scope_add("r_ECEF", &c30_r_ECEF, c30_sf_marshall);
sf_debug_symbol_scope_add("t", &c30_t, c30_b_sf_marshall);
sf_debug_symbol_scope_add("r_I", &c30_r_I, c30_sf_marshall);
CV_EML_FCN(0, 0);
/* u: position vector in ECI (in m) */
/* y: position vector in ECEF (in m) */
_SFD_EML_CALL(0,5);
c30_stperut = 1.0027379093500000E+000;
/* siderial time = stperut * universal time */
_SFD_EML_CALL(0,6);
c30_W_EARTH_ROT = 7.2940705438520400E-005;
/* sidereal rotation angular velocity of earth, SI */
_SFD_EML_CALL(0,8);
c30_ut_sec = 1.8225E+007 + c30_t;
/* universal time vector in sec */
_SFD_EML_CALL(0,9);
c30_b = c30_ut_sec;
c30_st_sec = 1.0027379093500000E+000 * c30_b;
/* sidereal time vector in sec */
_SFD_EML_CALL(0,11);
c30_a = c30_st_sec;
c30_phi = c30_a * 7.2940705438520400E-005;
/* sidereal time vector in rad */
_SFD_EML_CALL(0,13);
c30_x = c30_phi;
c30_b_x = c30_x;
c30_c_x = c30_b_x;
c30_b_x = c30_c_x;
c30_b_x = muDoubleScalarCos(c30_b_x);
c30_d_x = c30_phi;
c30_e_x = c30_d_x;
c30_f_x = c30_e_x;
c30_e_x = c30_f_x;
c30_e_x = muDoubleScalarSin(c30_e_x);
c30_g_x = c30_phi;
c30_h_x = c30_g_x;
c30_i_x = c30_h_x;
c30_h_x = c30_i_x;
c30_h_x = muDoubleScalarSin(c30_h_x);
c30_j_x = c30_phi;
c30_k_x = c30_j_x;
c30_l_x = c30_k_x;
c30_k_x = c30_l_x;
c30_k_x = muDoubleScalarCos(c30_k_x);
c30_TEI[0] = c30_k_x;
c30_TEI[3] = c30_h_x;
c30_TEI[6] = 0.0;
c30_TEI[1] = -c30_e_x;
c30_TEI[4] = c30_b_x;
c30_TEI[7] = 0.0;
c30_i6 = 0;
for (c30_i7 = 0; c30_i7 < 3; c30_i7 = c30_i7 + 1) {
c30_TEI[c30_i6 + 2] = c30_dv1[c30_i7];
c30_i6 = c30_i6 + 3;
}
_SFD_EML_CALL(0,17);
for (c30_i8 = 0; c30_i8 < 9; c30_i8 = c30_i8 + 1) {
c30_b_a[c30_i8] = c30_TEI[c30_i8];
}
for (c30_i9 = 0; c30_i9 < 3; c30_i9 = c30_i9 + 1) {
c30_b_b[c30_i9] = c30_r_I[c30_i9];
}
c30_eml_scalar_eg();
c30_eml_scalar_eg();
for (c30_i10 = 0; c30_i10 < 9; c30_i10 = c30_i10 + 1) {
c30_A[c30_i10] = c30_b_a[c30_i10];
}
for (c30_i11 = 0; c30_i11 < 3; c30_i11 = c30_i11 + 1) {
c30_B[c30_i11] = c30_b_b[c30_i11];
}
for (c30_i12 = 0; c30_i12 < 3; c30_i12 = c30_i12 + 1) {
c30_r_ECEF[c30_i12] = 0.0;
}
for (c30_i13 = 0; c30_i13 < 9; c30_i13 = c30_i13 + 1) {
c30_b_A[c30_i13] = c30_A[c30_i13];
}
for (c30_i14 = 0; c30_i14 < 3; c30_i14 = c30_i14 + 1) {
c30_b_B[c30_i14] = c30_B[c30_i14];
}
for (c30_i15 = 0; c30_i15 < 9; c30_i15 = c30_i15 + 1) {
c30_c_A[c30_i15] = c30_b_A[c30_i15];
}
for (c30_i16 = 0; c30_i16 < 3; c30_i16 = c30_i16 + 1) {
c30_c_B[c30_i16] = c30_b_B[c30_i16];
}
for (c30_i17 = 0; c30_i17 < 3; c30_i17 = c30_i17 + 1) {
c30_r_ECEF[c30_i17] = 0.0;
c30_i18 = 0;
for (c30_i19 = 0; c30_i19 < 3; c30_i19 = c30_i19 + 1) {
c30_r_ECEF[c30_i17] = c30_r_ECEF[c30_i17] + c30_c_A[c30_i18 + c30_i17] *
c30_c_B[c30_i19];
c30_i18 = c30_i18 + 3;
}
}
_SFD_EML_CALL(0,-17);
sf_debug_symbol_scope_pop();
for (c30_i20 = 0; c30_i20 < 3; c30_i20 = c30_i20 + 1) {
(*c30_b_r_ECEF)[c30_i20] = c30_r_ECEF[c30_i20];
}
_SFD_CC_CALL(EXIT_OUT_OF_FUNCTION_TAG,23);
}
static void sf_gateway_c7_CSE1_DP(SFc7_CSE1_DPInstanceStruct *chartInstance)
{
real_T c7_hoistedGlobal;
real_T c7_b_hoistedGlobal;
real_T c7_alpha;
real_T c7_u_VST;
uint32_T c7_debug_family_var_map[24];
real_T c7_a1;
real_T c7_b1;
real_T c7_c1;
real_T c7_a2;
real_T c7_b2;
real_T c7_c2;
real_T c7_a3;
real_T c7_b3;
real_T c7_c3;
real_T c7_a4;
real_T c7_b4;
real_T c7_c4;
real_T c7_a5;
real_T c7_b5;
real_T c7_c5;
real_T c7_a6;
real_T c7_b6;
real_T c7_c6;
real_T c7_nargin = 2.0;
real_T c7_nargout = 2.0;
real_T c7_F_x;
real_T c7_F_y;
real_T c7_x;
real_T c7_b_x;
real_T c7_c_x;
real_T c7_d_x;
real_T c7_e_x;
real_T c7_f_x;
real_T c7_g_x;
real_T c7_h_x;
real_T c7_i_x;
real_T c7_j_x;
real_T c7_k_x;
real_T c7_l_x;
real_T c7_m_x;
real_T c7_n_x;
real_T c7_o_x;
real_T c7_p_x;
real_T c7_q_x;
real_T c7_r_x;
real_T c7_s_x;
real_T c7_t_x;
real_T c7_u_x;
real_T c7_v_x;
real_T c7_w_x;
real_T c7_x_x;
real_T *c7_b_alpha;
real_T *c7_b_F_x;
real_T *c7_b_u_VST;
real_T *c7_b_F_y;
c7_b_F_y = (real_T *)ssGetOutputPortSignal(chartInstance->S, 2);
c7_b_u_VST = (real_T *)ssGetInputPortSignal(chartInstance->S, 1);
c7_b_F_x = (real_T *)ssGetOutputPortSignal(chartInstance->S, 1);
c7_b_alpha = (real_T *)ssGetInputPortSignal(chartInstance->S, 0);
_SFD_SYMBOL_SCOPE_PUSH(0U, 0U);
_sfTime_ = sf_get_time(chartInstance->S);
_SFD_CC_CALL(CHART_ENTER_SFUNCTION_TAG, 6U, chartInstance->c7_sfEvent);
_SFD_DATA_RANGE_CHECK(*c7_b_alpha, 0U);
chartInstance->c7_sfEvent = CALL_EVENT;
_SFD_CC_CALL(CHART_ENTER_DURING_FUNCTION_TAG, 6U, chartInstance->c7_sfEvent);
c7_hoistedGlobal = *c7_b_alpha;
c7_b_hoistedGlobal = *c7_b_u_VST;
c7_alpha = c7_hoistedGlobal;
c7_u_VST = c7_b_hoistedGlobal;
_SFD_SYMBOL_SCOPE_PUSH_EML(0U, 24U, 24U, c7_debug_family_names,
c7_debug_family_var_map);
_SFD_SYMBOL_SCOPE_ADD_EML(&c7_a1, 0U, c7_sf_marshallOut);
_SFD_SYMBOL_SCOPE_ADD_EML(&c7_b1, 1U, c7_sf_marshallOut);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c7_c1, 2U, c7_sf_marshallOut,
c7_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML(&c7_a2, 3U, c7_sf_marshallOut);
_SFD_SYMBOL_SCOPE_ADD_EML(&c7_b2, 4U, c7_sf_marshallOut);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c7_c2, 5U, c7_sf_marshallOut,
c7_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML(&c7_a3, 6U, c7_sf_marshallOut);
_SFD_SYMBOL_SCOPE_ADD_EML(&c7_b3, 7U, c7_sf_marshallOut);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c7_c3, 8U, c7_sf_marshallOut,
c7_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML(&c7_a4, 9U, c7_sf_marshallOut);
_SFD_SYMBOL_SCOPE_ADD_EML(&c7_b4, 10U, c7_sf_marshallOut);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c7_c4, 11U, c7_sf_marshallOut,
c7_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML(&c7_a5, 12U, c7_sf_marshallOut);
_SFD_SYMBOL_SCOPE_ADD_EML(&c7_b5, 13U, c7_sf_marshallOut);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c7_c5, 14U, c7_sf_marshallOut,
c7_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML(&c7_a6, 15U, c7_sf_marshallOut);
_SFD_SYMBOL_SCOPE_ADD_EML(&c7_b6, 16U, c7_sf_marshallOut);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c7_c6, 17U, c7_sf_marshallOut,
c7_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c7_nargin, 18U, c7_sf_marshallOut,
c7_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c7_nargout, 19U, c7_sf_marshallOut,
c7_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML(&c7_alpha, 20U, c7_sf_marshallOut);
_SFD_SYMBOL_SCOPE_ADD_EML(&c7_u_VST, 21U, c7_sf_marshallOut);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c7_F_x, 22U, c7_sf_marshallOut,
c7_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c7_F_y, 23U, c7_sf_marshallOut,
c7_sf_marshallIn);
CV_EML_FCN(0, 0);
_SFD_EML_CALL(0U, chartInstance->c7_sfEvent, 4);
c7_a1 = 1.107;
_SFD_EML_CALL(0U, chartInstance->c7_sfEvent, 5);
c7_b1 = 1.001;
_SFD_EML_CALL(0U, chartInstance->c7_sfEvent, 6);
c7_c1 = 1.564;
_SFD_EML_CALL(0U, chartInstance->c7_sfEvent, 7);
c7_a2 = 0.3471;
_SFD_EML_CALL(0U, chartInstance->c7_sfEvent, 8);
c7_b2 = 0.1397;
_SFD_EML_CALL(0U, chartInstance->c7_sfEvent, 9);
c7_c2 = -1.599;
_SFD_EML_CALL(0U, chartInstance->c7_sfEvent, 10);
c7_a3 = 0.1558;
_SFD_EML_CALL(0U, chartInstance->c7_sfEvent, 11);
c7_b3 = 1.993;
_SFD_EML_CALL(0U, chartInstance->c7_sfEvent, 12);
c7_c3 = 1.886;
_SFD_EML_CALL(0U, chartInstance->c7_sfEvent, 13);
c7_a4 = 0.1599;
_SFD_EML_CALL(0U, chartInstance->c7_sfEvent, 14);
c7_b4 = 0.218;
_SFD_EML_CALL(0U, chartInstance->c7_sfEvent, 15);
c7_c4 = 1.513;
_SFD_EML_CALL(0U, chartInstance->c7_sfEvent, 16);
c7_a5 = 0.08237;
_SFD_EML_CALL(0U, chartInstance->c7_sfEvent, 17);
c7_b5 = 2.986;
_SFD_EML_CALL(0U, chartInstance->c7_sfEvent, 18);
c7_c5 = -1.601;
_SFD_EML_CALL(0U, chartInstance->c7_sfEvent, 19);
c7_a6 = 0.04641;
_SFD_EML_CALL(0U, chartInstance->c7_sfEvent, 20);
c7_b6 = 3.964;
_SFD_EML_CALL(0U, chartInstance->c7_sfEvent, 21);
c7_c6 = 1.738;
_SFD_EML_CALL(0U, chartInstance->c7_sfEvent, 22);
c7_x = 1.001 * c7_alpha + c7_c1;
c7_b_x = c7_x;
c7_b_x = muDoubleScalarSin(c7_b_x);
c7_c_x = 0.1397 * c7_alpha + c7_c2;
c7_d_x = c7_c_x;
c7_d_x = muDoubleScalarSin(c7_d_x);
c7_e_x = 1.993 * c7_alpha + c7_c3;
c7_f_x = c7_e_x;
c7_f_x = muDoubleScalarSin(c7_f_x);
c7_g_x = 0.218 * c7_alpha + c7_c4;
c7_h_x = c7_g_x;
c7_h_x = muDoubleScalarSin(c7_h_x);
c7_i_x = 2.986 * c7_alpha + c7_c5;
c7_j_x = c7_i_x;
c7_j_x = muDoubleScalarSin(c7_j_x);
c7_k_x = 3.964 * c7_alpha + c7_c6;
c7_l_x = c7_k_x;
c7_l_x = muDoubleScalarSin(c7_l_x);
c7_F_x = c7_u_VST * (((((1.107 * c7_b_x + 0.3471 * c7_d_x) + 0.1558 * c7_f_x)
+ 0.1599 * c7_h_x) + 0.08237 * c7_j_x) + 0.04641 * c7_l_x);
_SFD_EML_CALL(0U, chartInstance->c7_sfEvent, 29);
c7_a1 = 0.7936;
_SFD_EML_CALL(0U, chartInstance->c7_sfEvent, 30);
c7_b1 = 1.001;
_SFD_EML_CALL(0U, chartInstance->c7_sfEvent, 31);
c7_c1 = 0.01605;
_SFD_EML_CALL(0U, chartInstance->c7_sfEvent, 32);
c7_a2 = 0.2112;
_SFD_EML_CALL(0U, chartInstance->c7_sfEvent, 33);
c7_b2 = 1.997;
_SFD_EML_CALL(0U, chartInstance->c7_sfEvent, 34);
c7_c2 = 0.03229;
_SFD_EML_CALL(0U, chartInstance->c7_sfEvent, 35);
c7_a3 = 0.07608;
_SFD_EML_CALL(0U, chartInstance->c7_sfEvent, 36);
c7_b3 = 2.991;
_SFD_EML_CALL(0U, chartInstance->c7_sfEvent, 37);
c7_c3 = 2.986;
_SFD_EML_CALL(0U, chartInstance->c7_sfEvent, 38);
c7_a4 = 0.04817;
_SFD_EML_CALL(0U, chartInstance->c7_sfEvent, 39);
c7_b4 = 3.999;
_SFD_EML_CALL(0U, chartInstance->c7_sfEvent, 40);
c7_c4 = -0.8668;
_SFD_EML_CALL(0U, chartInstance->c7_sfEvent, 41);
c7_a5 = 0.01757;
_SFD_EML_CALL(0U, chartInstance->c7_sfEvent, 42);
c7_b5 = 4.904;
_SFD_EML_CALL(0U, chartInstance->c7_sfEvent, 43);
c7_c5 = -2.622;
_SFD_EML_CALL(0U, chartInstance->c7_sfEvent, 44);
c7_a6 = -0.02089;
_SFD_EML_CALL(0U, chartInstance->c7_sfEvent, 45);
c7_b6 = 5.068;
_SFD_EML_CALL(0U, chartInstance->c7_sfEvent, 46);
c7_c6 = 0.2548;
_SFD_EML_CALL(0U, chartInstance->c7_sfEvent, 47);
c7_m_x = 1.001 * c7_alpha + c7_c1;
c7_n_x = c7_m_x;
c7_n_x = muDoubleScalarSin(c7_n_x);
c7_o_x = 1.997 * c7_alpha + c7_c2;
c7_p_x = c7_o_x;
c7_p_x = muDoubleScalarSin(c7_p_x);
c7_q_x = 2.991 * c7_alpha + c7_c3;
c7_r_x = c7_q_x;
c7_r_x = muDoubleScalarSin(c7_r_x);
c7_s_x = 3.999 * c7_alpha + c7_c4;
c7_t_x = c7_s_x;
c7_t_x = muDoubleScalarSin(c7_t_x);
c7_u_x = 4.904 * c7_alpha + c7_c5;
c7_v_x = c7_u_x;
c7_v_x = muDoubleScalarSin(c7_v_x);
c7_w_x = 5.068 * c7_alpha + c7_c6;
c7_x_x = c7_w_x;
c7_x_x = muDoubleScalarSin(c7_x_x);
c7_F_y = c7_u_VST * (((((0.7936 * c7_n_x + 0.2112 * c7_p_x) + 0.07608 * c7_r_x)
+ 0.04817 * c7_t_x) + 0.01757 * c7_v_x) + -0.02089 * c7_x_x);
_SFD_EML_CALL(0U, chartInstance->c7_sfEvent, -47);
_SFD_SYMBOL_SCOPE_POP();
*c7_b_F_x = c7_F_x;
*c7_b_F_y = c7_F_y;
_SFD_CC_CALL(EXIT_OUT_OF_FUNCTION_TAG, 6U, chartInstance->c7_sfEvent);
_SFD_SYMBOL_SCOPE_POP();
_SFD_CHECK_FOR_STATE_INCONSISTENCY(_CSE1_DPMachineNumber_,
chartInstance->chartNumber, chartInstance->instanceNumber);
_SFD_DATA_RANGE_CHECK(*c7_b_F_x, 1U);
_SFD_DATA_RANGE_CHECK(*c7_b_u_VST, 2U);
_SFD_DATA_RANGE_CHECK(*c7_b_F_y, 3U);
}
static void sf_c10_HIL_model_overall(SFc10_HIL_model_overallInstanceStruct
*chartInstance)
{
int32_T c10_i2;
int32_T c10_i3;
int32_T c10_i4;
real_T c10_eta[3];
uint32_T c10_debug_family_var_map[7];
real_T c10_psi;
real_T c10_c;
real_T c10_s;
real_T c10_nargin = 1.0;
real_T c10_nargout = 1.0;
real_T c10_R[9];
real_T c10_x;
real_T c10_b_x;
real_T c10_c_x;
real_T c10_d_x;
int32_T c10_i5;
int32_T c10_i6;
static real_T c10_dv1[3] = { 0.0, 0.0, 1.0 };
int32_T c10_i7;
real_T (*c10_b_R)[9];
real_T (*c10_b_eta)[3];
c10_b_R = (real_T (*)[9])ssGetOutputPortSignal(chartInstance->S, 1);
c10_b_eta = (real_T (*)[3])ssGetInputPortSignal(chartInstance->S, 0);
_sfTime_ = (real_T)ssGetT(chartInstance->S);
_SFD_CC_CALL(CHART_ENTER_SFUNCTION_TAG, 9U, chartInstance->c10_sfEvent);
for (c10_i2 = 0; c10_i2 < 3; c10_i2++) {
_SFD_DATA_RANGE_CHECK((*c10_b_eta)[c10_i2], 0U);
}
for (c10_i3 = 0; c10_i3 < 9; c10_i3++) {
_SFD_DATA_RANGE_CHECK((*c10_b_R)[c10_i3], 1U);
}
chartInstance->c10_sfEvent = CALL_EVENT;
_SFD_CC_CALL(CHART_ENTER_DURING_FUNCTION_TAG, 9U, chartInstance->c10_sfEvent);
for (c10_i4 = 0; c10_i4 < 3; c10_i4++) {
c10_eta[c10_i4] = (*c10_b_eta)[c10_i4];
}
_SFD_SYMBOL_SCOPE_PUSH_EML(0U, 7U, 7U, c10_debug_family_names,
c10_debug_family_var_map);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c10_psi, 0U, c10_c_sf_marshallOut,
c10_b_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c10_c, 1U, c10_c_sf_marshallOut,
c10_b_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c10_s, 2U, c10_c_sf_marshallOut,
c10_b_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c10_nargin, 3U, c10_c_sf_marshallOut,
c10_b_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c10_nargout, 4U, c10_c_sf_marshallOut,
c10_b_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML(c10_eta, 5U, c10_b_sf_marshallOut);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(c10_R, 6U, c10_sf_marshallOut,
c10_sf_marshallIn);
CV_EML_FCN(0, 0);
_SFD_EML_CALL(0U, chartInstance->c10_sfEvent, 3);
CV_EML_IF(0, 1, 0, TRUE);
_SFD_EML_CALL(0U, chartInstance->c10_sfEvent, 4);
c10_psi = c10_eta[2];
_SFD_EML_CALL(0U, chartInstance->c10_sfEvent, 9);
c10_x = c10_psi;
c10_c = c10_x;
c10_b_x = c10_c;
c10_c = c10_b_x;
c10_c = muDoubleScalarCos(c10_c);
_SFD_EML_CALL(0U, chartInstance->c10_sfEvent, 10);
c10_c_x = c10_psi;
c10_s = c10_c_x;
c10_d_x = c10_s;
c10_s = c10_d_x;
c10_s = muDoubleScalarSin(c10_s);
_SFD_EML_CALL(0U, chartInstance->c10_sfEvent, 12);
c10_R[0] = c10_c;
c10_R[3] = -c10_s;
c10_R[6] = 0.0;
c10_R[1] = c10_s;
c10_R[4] = c10_c;
c10_R[7] = 0.0;
c10_i5 = 0;
for (c10_i6 = 0; c10_i6 < 3; c10_i6++) {
c10_R[c10_i5 + 2] = c10_dv1[c10_i6];
c10_i5 += 3;
}
_SFD_EML_CALL(0U, chartInstance->c10_sfEvent, -12);
_SFD_SYMBOL_SCOPE_POP();
for (c10_i7 = 0; c10_i7 < 9; c10_i7++) {
(*c10_b_R)[c10_i7] = c10_R[c10_i7];
}
_SFD_CC_CALL(EXIT_OUT_OF_FUNCTION_TAG, 9U, chartInstance->c10_sfEvent);
_SFD_CHECK_FOR_STATE_INCONSISTENCY(_HIL_model_overallMachineNumber_,
chartInstance->chartNumber, chartInstance->instanceNumber);
}
static void sf_c14_Model_justmodel(SFc14_Model_justmodelInstanceStruct
*chartInstance)
{
int32_T c14_i4;
int32_T c14_i5;
int32_T c14_i6;
int32_T c14_i7;
real_T c14_eta_s_in[3];
uint32_T c14_debug_family_var_map[9];
real_T c14_eta_s[3];
real_T c14_psi;
real_T c14_c;
real_T c14_s;
real_T c14_nargin = 1.0;
real_T c14_nargout = 2.0;
real_T c14_Rs[9];
real_T c14_Rst[9];
int32_T c14_i8;
real_T c14_x;
real_T c14_b_x;
real_T c14_c_x;
real_T c14_d_x;
int32_T c14_i9;
int32_T c14_i10;
static real_T c14_dv2[3] = { 0.0, 0.0, 1.0 };
int32_T c14_i11;
int32_T c14_i12;
int32_T c14_i13;
int32_T c14_i14;
int32_T c14_i15;
int32_T c14_i16;
real_T (*c14_b_Rs)[9];
real_T (*c14_b_Rst)[9];
real_T (*c14_b_eta_s_in)[3];
c14_b_Rst = (real_T (*)[9])ssGetOutputPortSignal(chartInstance->S, 2);
c14_b_Rs = (real_T (*)[9])ssGetOutputPortSignal(chartInstance->S, 1);
c14_b_eta_s_in = (real_T (*)[3])ssGetInputPortSignal(chartInstance->S, 0);
_sfTime_ = (real_T)ssGetT(chartInstance->S);
_SFD_CC_CALL(CHART_ENTER_SFUNCTION_TAG, 13U, chartInstance->c14_sfEvent);
for (c14_i4 = 0; c14_i4 < 3; c14_i4++) {
_SFD_DATA_RANGE_CHECK((*c14_b_eta_s_in)[c14_i4], 0U);
}
for (c14_i5 = 0; c14_i5 < 9; c14_i5++) {
_SFD_DATA_RANGE_CHECK((*c14_b_Rs)[c14_i5], 1U);
}
for (c14_i6 = 0; c14_i6 < 9; c14_i6++) {
_SFD_DATA_RANGE_CHECK((*c14_b_Rst)[c14_i6], 2U);
}
chartInstance->c14_sfEvent = CALL_EVENT;
_SFD_CC_CALL(CHART_ENTER_DURING_FUNCTION_TAG, 13U, chartInstance->c14_sfEvent);
for (c14_i7 = 0; c14_i7 < 3; c14_i7++) {
c14_eta_s_in[c14_i7] = (*c14_b_eta_s_in)[c14_i7];
}
_SFD_SYMBOL_SCOPE_PUSH_EML(0U, 9U, 9U, c14_debug_family_names,
c14_debug_family_var_map);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(c14_eta_s, 0U, c14_b_sf_marshallOut,
c14_c_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c14_psi, 1U, c14_c_sf_marshallOut,
c14_b_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c14_c, 2U, c14_c_sf_marshallOut,
c14_b_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c14_s, 3U, c14_c_sf_marshallOut,
c14_b_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c14_nargin, 4U, c14_c_sf_marshallOut,
c14_b_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c14_nargout, 5U, c14_c_sf_marshallOut,
c14_b_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML(c14_eta_s_in, 6U, c14_b_sf_marshallOut);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(c14_Rs, 7U, c14_sf_marshallOut,
c14_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(c14_Rst, 8U, c14_sf_marshallOut,
c14_sf_marshallIn);
CV_EML_FCN(0, 0);
_SFD_EML_CALL(0U, chartInstance->c14_sfEvent, 3);
CV_EML_IF(0, 1, 0, TRUE);
_SFD_EML_CALL(0U, chartInstance->c14_sfEvent, 4);
for (c14_i8 = 0; c14_i8 < 3; c14_i8++) {
c14_eta_s[c14_i8] = c14_eta_s_in[c14_i8];
}
_SFD_EML_CALL(0U, chartInstance->c14_sfEvent, 9);
c14_psi = c14_eta_s[2];
_SFD_EML_CALL(0U, chartInstance->c14_sfEvent, 11);
c14_x = c14_psi;
c14_c = c14_x;
c14_b_x = c14_c;
c14_c = c14_b_x;
c14_c = muDoubleScalarCos(c14_c);
_SFD_EML_CALL(0U, chartInstance->c14_sfEvent, 12);
c14_c_x = c14_psi;
c14_s = c14_c_x;
c14_d_x = c14_s;
c14_s = c14_d_x;
c14_s = muDoubleScalarSin(c14_s);
_SFD_EML_CALL(0U, chartInstance->c14_sfEvent, 14);
c14_Rs[0] = c14_c;
c14_Rs[3] = -c14_s;
c14_Rs[6] = 0.0;
c14_Rs[1] = c14_s;
c14_Rs[4] = c14_c;
c14_Rs[7] = 0.0;
c14_i9 = 0;
for (c14_i10 = 0; c14_i10 < 3; c14_i10++) {
c14_Rs[c14_i9 + 2] = c14_dv2[c14_i10];
c14_i9 += 3;
}
_SFD_EML_CALL(0U, chartInstance->c14_sfEvent, 17);
c14_i11 = 0;
for (c14_i12 = 0; c14_i12 < 3; c14_i12++) {
c14_i13 = 0;
for (c14_i14 = 0; c14_i14 < 3; c14_i14++) {
c14_Rst[c14_i14 + c14_i11] = c14_Rs[c14_i13 + c14_i12];
c14_i13 += 3;
}
c14_i11 += 3;
}
_SFD_EML_CALL(0U, chartInstance->c14_sfEvent, -17);
_SFD_SYMBOL_SCOPE_POP();
for (c14_i15 = 0; c14_i15 < 9; c14_i15++) {
(*c14_b_Rs)[c14_i15] = c14_Rs[c14_i15];
}
for (c14_i16 = 0; c14_i16 < 9; c14_i16++) {
(*c14_b_Rst)[c14_i16] = c14_Rst[c14_i16];
}
_SFD_CC_CALL(EXIT_OUT_OF_FUNCTION_TAG, 13U, chartInstance->c14_sfEvent);
_SFD_CHECK_FOR_STATE_INCONSISTENCY(_Model_justmodelMachineNumber_,
chartInstance->chartNumber, chartInstance->instanceNumber);
}
static void sf_c20_ekffede(SFc20_ekffedeInstanceStruct *chartInstance)
{
int32_T c20_i0;
real_T c20_hoistedGlobal;
real_T c20_b_hoistedGlobal;
int32_T c20_i1;
real_T c20_mag[3];
real_T c20_phi;
real_T c20_theta;
uint32_T c20_debug_family_var_map[8];
real_T c20_mn;
real_T c20_me;
real_T c20_nargin = 3.0;
real_T c20_nargout = 1.0;
real_T c20_psi;
real_T c20_x;
real_T c20_b_x;
real_T c20_a;
real_T c20_b;
real_T c20_y;
real_T c20_c_x;
real_T c20_d_x;
real_T c20_e_x;
real_T c20_f_x;
real_T c20_b_a;
real_T c20_b_b;
real_T c20_b_y;
real_T c20_c_a;
real_T c20_c_b;
real_T c20_c_y;
real_T c20_g_x;
real_T c20_h_x;
real_T c20_i_x;
real_T c20_j_x;
real_T c20_d_a;
real_T c20_d_b;
real_T c20_d_y;
real_T c20_e_a;
real_T c20_e_b;
real_T c20_e_y;
real_T c20_k_x;
real_T c20_l_x;
real_T c20_f_a;
real_T c20_f_b;
real_T c20_f_y;
real_T c20_m_x;
real_T c20_n_x;
real_T c20_g_a;
real_T c20_g_b;
real_T c20_g_y;
real_T c20_h_y;
real_T c20_o_x;
real_T c20_i_y;
real_T c20_p_x;
real_T c20_r;
real_T *c20_b_psi;
real_T *c20_b_phi;
real_T *c20_b_theta;
real_T (*c20_b_mag)[3];
c20_b_theta = (real_T *)ssGetInputPortSignal(chartInstance->S, 2);
c20_b_phi = (real_T *)ssGetInputPortSignal(chartInstance->S, 1);
c20_b_psi = (real_T *)ssGetOutputPortSignal(chartInstance->S, 1);
c20_b_mag = (real_T (*)[3])ssGetInputPortSignal(chartInstance->S, 0);
_sfTime_ = (real_T)ssGetT(chartInstance->S);
_SFD_CC_CALL(CHART_ENTER_SFUNCTION_TAG, 19U, chartInstance->c20_sfEvent);
for (c20_i0 = 0; c20_i0 < 3; c20_i0++) {
_SFD_DATA_RANGE_CHECK((*c20_b_mag)[c20_i0], 0U);
}
_SFD_DATA_RANGE_CHECK(*c20_b_psi, 1U);
_SFD_DATA_RANGE_CHECK(*c20_b_phi, 2U);
_SFD_DATA_RANGE_CHECK(*c20_b_theta, 3U);
chartInstance->c20_sfEvent = CALL_EVENT;
_SFD_CC_CALL(CHART_ENTER_DURING_FUNCTION_TAG, 19U, chartInstance->c20_sfEvent);
c20_hoistedGlobal = *c20_b_phi;
c20_b_hoistedGlobal = *c20_b_theta;
for (c20_i1 = 0; c20_i1 < 3; c20_i1++) {
c20_mag[c20_i1] = (*c20_b_mag)[c20_i1];
}
c20_phi = c20_hoistedGlobal;
c20_theta = c20_b_hoistedGlobal;
sf_debug_symbol_scope_push_eml(0U, 8U, 8U, c20_debug_family_names,
c20_debug_family_var_map);
sf_debug_symbol_scope_add_eml_importable(&c20_mn, 0U, c20_sf_marshallOut,
c20_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c20_me, 1U, c20_sf_marshallOut,
c20_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c20_nargin, 2U, c20_sf_marshallOut,
c20_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c20_nargout, 3U, c20_sf_marshallOut,
c20_sf_marshallIn);
sf_debug_symbol_scope_add_eml(c20_mag, 4U, c20_b_sf_marshallOut);
sf_debug_symbol_scope_add_eml(&c20_phi, 5U, c20_sf_marshallOut);
sf_debug_symbol_scope_add_eml(&c20_theta, 6U, c20_sf_marshallOut);
sf_debug_symbol_scope_add_eml_importable(&c20_psi, 7U, c20_sf_marshallOut,
c20_sf_marshallIn);
CV_EML_FCN(0, 0);
_SFD_EML_CALL(0U, chartInstance->c20_sfEvent, 3);
c20_x = c20_theta;
c20_b_x = c20_x;
c20_b_x = muDoubleScalarCos(c20_b_x);
c20_a = c20_b_x;
c20_b = c20_mag[0];
c20_y = c20_a * c20_b;
c20_c_x = c20_phi;
c20_d_x = c20_c_x;
c20_d_x = muDoubleScalarSin(c20_d_x);
c20_e_x = c20_theta;
c20_f_x = c20_e_x;
c20_f_x = muDoubleScalarSin(c20_f_x);
c20_b_a = c20_d_x;
c20_b_b = c20_f_x;
c20_b_y = c20_b_a * c20_b_b;
c20_c_a = c20_b_y;
c20_c_b = c20_mag[1];
c20_c_y = c20_c_a * c20_c_b;
c20_g_x = c20_phi;
c20_h_x = c20_g_x;
c20_h_x = muDoubleScalarCos(c20_h_x);
c20_i_x = c20_theta;
c20_j_x = c20_i_x;
c20_j_x = muDoubleScalarSin(c20_j_x);
c20_d_a = c20_h_x;
c20_d_b = c20_j_x;
c20_d_y = c20_d_a * c20_d_b;
c20_e_a = c20_d_y;
c20_e_b = c20_mag[2];
c20_e_y = c20_e_a * c20_e_b;
c20_mn = (c20_y + c20_c_y) + c20_e_y;
_SFD_EML_CALL(0U, chartInstance->c20_sfEvent, 7);
c20_k_x = c20_phi;
c20_l_x = c20_k_x;
c20_l_x = muDoubleScalarCos(c20_l_x);
c20_f_a = c20_l_x;
c20_f_b = c20_mag[1];
c20_f_y = c20_f_a * c20_f_b;
c20_m_x = c20_phi;
c20_n_x = c20_m_x;
c20_n_x = muDoubleScalarSin(c20_n_x);
c20_g_a = -c20_n_x;
c20_g_b = c20_mag[2];
c20_g_y = c20_g_a * c20_g_b;
c20_me = c20_f_y + c20_g_y;
_SFD_EML_CALL(0U, chartInstance->c20_sfEvent, 10);
c20_h_y = c20_me;
c20_o_x = c20_mn;
c20_i_y = c20_h_y;
c20_p_x = c20_o_x;
c20_r = muDoubleScalarAtan2(c20_i_y, c20_p_x);
c20_psi = -c20_r;
_SFD_EML_CALL(0U, chartInstance->c20_sfEvent, -10);
sf_debug_symbol_scope_pop();
*c20_b_psi = c20_psi;
_SFD_CC_CALL(EXIT_OUT_OF_FUNCTION_TAG, 19U, chartInstance->c20_sfEvent);
sf_debug_check_for_state_inconsistency(_ekffedeMachineNumber_,
chartInstance->chartNumber, chartInstance->instanceNumber);
}
static void sf_c16_HIL_model_overall(SFc16_HIL_model_overallInstanceStruct
*chartInstance)
{
int32_T c16_i2;
int32_T c16_i3;
real_T c16_hoistedGlobal;
real_T c16_b_hoistedGlobal;
real_T c16_c_hoistedGlobal;
int32_T c16_i4;
real_T c16_WP[4];
real_T c16_L_rope;
real_T c16_L_fast;
real_T c16_Offset_direction;
uint32_T c16_debug_family_var_map[17];
real_T c16_x1;
real_T c16_y1;
real_T c16_x2;
real_T c16_y2;
real_T c16_beta;
real_T c16_alpha;
real_T c16_L;
real_T c16_x;
real_T c16_y;
real_T c16_psi;
real_T c16_nargin = 4.0;
real_T c16_nargout = 1.0;
real_T c16_eta_ref[3];
real_T c16_b_y;
real_T c16_b_x;
real_T c16_c_y;
real_T c16_c_x;
real_T c16_a;
real_T c16_d_y;
real_T c16_A;
real_T c16_d_x;
real_T c16_e_x;
real_T c16_e_y;
real_T c16_f_x;
real_T c16_g_x;
real_T c16_b_a;
real_T c16_b;
real_T c16_f_y;
real_T c16_h_x;
real_T c16_i_x;
real_T c16_c_a;
real_T c16_b_b;
real_T c16_g_y;
int32_T c16_i5;
real_T *c16_b_Offset_direction;
real_T *c16_b_L_fast;
real_T *c16_b_L_rope;
real_T (*c16_b_eta_ref)[3];
real_T (*c16_b_WP)[4];
c16_b_Offset_direction = (real_T *)ssGetInputPortSignal(chartInstance->S, 3);
c16_b_L_fast = (real_T *)ssGetInputPortSignal(chartInstance->S, 2);
c16_b_L_rope = (real_T *)ssGetInputPortSignal(chartInstance->S, 1);
c16_b_eta_ref = (real_T (*)[3])ssGetOutputPortSignal(chartInstance->S, 1);
c16_b_WP = (real_T (*)[4])ssGetInputPortSignal(chartInstance->S, 0);
_sfTime_ = (real_T)ssGetT(chartInstance->S);
_SFD_CC_CALL(CHART_ENTER_SFUNCTION_TAG, 15U, chartInstance->c16_sfEvent);
for (c16_i2 = 0; c16_i2 < 4; c16_i2++) {
_SFD_DATA_RANGE_CHECK((*c16_b_WP)[c16_i2], 0U);
}
for (c16_i3 = 0; c16_i3 < 3; c16_i3++) {
_SFD_DATA_RANGE_CHECK((*c16_b_eta_ref)[c16_i3], 1U);
}
_SFD_DATA_RANGE_CHECK(*c16_b_L_rope, 2U);
_SFD_DATA_RANGE_CHECK(*c16_b_L_fast, 3U);
_SFD_DATA_RANGE_CHECK(*c16_b_Offset_direction, 4U);
chartInstance->c16_sfEvent = CALL_EVENT;
_SFD_CC_CALL(CHART_ENTER_DURING_FUNCTION_TAG, 15U, chartInstance->c16_sfEvent);
c16_hoistedGlobal = *c16_b_L_rope;
c16_b_hoistedGlobal = *c16_b_L_fast;
c16_c_hoistedGlobal = *c16_b_Offset_direction;
for (c16_i4 = 0; c16_i4 < 4; c16_i4++) {
c16_WP[c16_i4] = (*c16_b_WP)[c16_i4];
}
c16_L_rope = c16_hoistedGlobal;
c16_L_fast = c16_b_hoistedGlobal;
c16_Offset_direction = c16_c_hoistedGlobal;
_SFD_SYMBOL_SCOPE_PUSH_EML(0U, 17U, 17U, c16_debug_family_names,
c16_debug_family_var_map);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c16_x1, 0U, c16_b_sf_marshallOut,
c16_b_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c16_y1, 1U, c16_b_sf_marshallOut,
c16_b_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c16_x2, 2U, c16_b_sf_marshallOut,
c16_b_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c16_y2, 3U, c16_b_sf_marshallOut,
c16_b_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c16_beta, 4U, c16_b_sf_marshallOut,
c16_b_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c16_alpha, 5U, c16_b_sf_marshallOut,
c16_b_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c16_L, 6U, c16_b_sf_marshallOut,
c16_b_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c16_x, 7U, c16_b_sf_marshallOut,
c16_b_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c16_y, 8U, c16_b_sf_marshallOut,
c16_b_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c16_psi, 9U, c16_b_sf_marshallOut,
c16_b_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c16_nargin, 10U, c16_b_sf_marshallOut,
c16_b_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c16_nargout, 11U, c16_b_sf_marshallOut,
c16_b_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML(c16_WP, 12U, c16_c_sf_marshallOut);
_SFD_SYMBOL_SCOPE_ADD_EML(&c16_L_rope, 13U, c16_b_sf_marshallOut);
_SFD_SYMBOL_SCOPE_ADD_EML(&c16_L_fast, 14U, c16_b_sf_marshallOut);
_SFD_SYMBOL_SCOPE_ADD_EML(&c16_Offset_direction, 15U, c16_b_sf_marshallOut);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(c16_eta_ref, 16U, c16_sf_marshallOut,
c16_sf_marshallIn);
CV_EML_FCN(0, 0);
_SFD_EML_CALL(0U, chartInstance->c16_sfEvent, 4);
c16_x1 = c16_WP[0];
_SFD_EML_CALL(0U, chartInstance->c16_sfEvent, 5);
c16_y1 = c16_WP[2];
_SFD_EML_CALL(0U, chartInstance->c16_sfEvent, 7);
c16_x2 = c16_WP[1];
_SFD_EML_CALL(0U, chartInstance->c16_sfEvent, 8);
c16_y2 = c16_WP[3];
_SFD_EML_CALL(0U, chartInstance->c16_sfEvent, 10);
c16_b_y = c16_y2 - c16_y1;
c16_b_x = c16_x2 - c16_x1;
c16_c_y = c16_b_y;
c16_c_x = c16_b_x;
c16_beta = muDoubleScalarAtan2(c16_c_y, c16_c_x);
_SFD_EML_CALL(0U, chartInstance->c16_sfEvent, 11);
c16_a = c16_Offset_direction;
c16_d_y = c16_a * 3.1415926535897931;
c16_A = c16_d_y;
c16_d_x = c16_A;
c16_e_x = c16_d_x;
c16_e_y = c16_e_x / 10.0;
c16_alpha = c16_beta + c16_e_y;
_SFD_EML_CALL(0U, chartInstance->c16_sfEvent, 13);
c16_L = c16_L_rope + c16_L_fast;
_SFD_EML_CALL(0U, chartInstance->c16_sfEvent, 14);
c16_f_x = c16_alpha;
c16_g_x = c16_f_x;
c16_g_x = muDoubleScalarCos(c16_g_x);
c16_b_a = c16_L;
c16_b = c16_g_x;
c16_f_y = c16_b_a * c16_b;
c16_x = c16_f_y + c16_x1;
_SFD_EML_CALL(0U, chartInstance->c16_sfEvent, 15);
c16_h_x = c16_alpha;
c16_i_x = c16_h_x;
c16_i_x = muDoubleScalarSin(c16_i_x);
c16_c_a = c16_L;
c16_b_b = c16_i_x;
c16_g_y = c16_c_a * c16_b_b;
c16_y = c16_g_y + c16_y1;
_SFD_EML_CALL(0U, chartInstance->c16_sfEvent, 16);
c16_psi = c16_alpha;
_SFD_EML_CALL(0U, chartInstance->c16_sfEvent, 18);
c16_eta_ref[0] = c16_x;
c16_eta_ref[1] = c16_y;
c16_eta_ref[2] = c16_psi;
_SFD_EML_CALL(0U, chartInstance->c16_sfEvent, -18);
_SFD_SYMBOL_SCOPE_POP();
for (c16_i5 = 0; c16_i5 < 3; c16_i5++) {
(*c16_b_eta_ref)[c16_i5] = c16_eta_ref[c16_i5];
}
_SFD_CC_CALL(EXIT_OUT_OF_FUNCTION_TAG, 15U, chartInstance->c16_sfEvent);
_SFD_CHECK_FOR_STATE_INCONSISTENCY(_HIL_model_overallMachineNumber_,
chartInstance->chartNumber, chartInstance->instanceNumber);
}
static void c26_chartstep_c26_HIL_model_experiments2
(SFc26_HIL_model_experiments2InstanceStruct *chartInstance)
{
int32_T c26_i5;
real_T c26_eta_ref[3];
int32_T c26_i6;
real_T c26_nu_ref[3];
uint32_T c26_debug_family_var_map[10];
real_T c26_psi_ref;
real_T c26_nuRef[3];
real_T c26_c;
real_T c26_s;
real_T c26_R[9];
real_T c26_nargin = 2.0;
real_T c26_nargout = 1.0;
real_T c26_eta_dot_ref[3];
int32_T c26_i7;
real_T c26_x;
real_T c26_b_x;
real_T c26_c_x;
real_T c26_d_x;
int32_T c26_i8;
int32_T c26_i9;
static real_T c26_dv1[3] = { 0.0, 0.0, 1.0 };
int32_T c26_i10;
real_T c26_a[9];
int32_T c26_i11;
real_T c26_b[3];
int32_T c26_i12;
int32_T c26_i13;
int32_T c26_i14;
real_T c26_C[3];
int32_T c26_i15;
int32_T c26_i16;
int32_T c26_i17;
int32_T c26_i18;
int32_T c26_i19;
int32_T c26_i20;
int32_T c26_i21;
real_T (*c26_b_eta_dot_ref)[3];
real_T (*c26_b_nu_ref)[3];
real_T (*c26_b_eta_ref)[3];
c26_b_nu_ref = (real_T (*)[3])ssGetInputPortSignal(chartInstance->S, 1);
c26_b_eta_dot_ref = (real_T (*)[3])ssGetOutputPortSignal(chartInstance->S, 1);
c26_b_eta_ref = (real_T (*)[3])ssGetInputPortSignal(chartInstance->S, 0);
_SFD_CC_CALL(CHART_ENTER_DURING_FUNCTION_TAG, 14U, chartInstance->c26_sfEvent);
for (c26_i5 = 0; c26_i5 < 3; c26_i5++) {
c26_eta_ref[c26_i5] = (*c26_b_eta_ref)[c26_i5];
}
for (c26_i6 = 0; c26_i6 < 3; c26_i6++) {
c26_nu_ref[c26_i6] = (*c26_b_nu_ref)[c26_i6];
}
_SFD_SYMBOL_SCOPE_PUSH_EML(0U, 10U, 10U, c26_debug_family_names,
c26_debug_family_var_map);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c26_psi_ref, 0U, c26_c_sf_marshallOut,
c26_b_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(c26_nuRef, 1U, c26_sf_marshallOut,
c26_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c26_c, 2U, c26_c_sf_marshallOut,
c26_b_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c26_s, 3U, c26_c_sf_marshallOut,
c26_b_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(c26_R, 4U, c26_d_sf_marshallOut,
c26_c_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c26_nargin, 5U, c26_c_sf_marshallOut,
c26_b_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c26_nargout, 6U, c26_c_sf_marshallOut,
c26_b_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML(c26_eta_ref, 7U, c26_b_sf_marshallOut);
_SFD_SYMBOL_SCOPE_ADD_EML(c26_nu_ref, 8U, c26_sf_marshallOut);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(c26_eta_dot_ref, 9U, c26_sf_marshallOut,
c26_sf_marshallIn);
CV_EML_FCN(0, 0);
_SFD_EML_CALL(0U, chartInstance->c26_sfEvent, 3);
CV_EML_IF(0, 1, 0, TRUE);
_SFD_EML_CALL(0U, chartInstance->c26_sfEvent, 4);
c26_psi_ref = c26_eta_ref[2];
_SFD_EML_CALL(0U, chartInstance->c26_sfEvent, 9);
CV_EML_IF(0, 1, 1, TRUE);
_SFD_EML_CALL(0U, chartInstance->c26_sfEvent, 10);
for (c26_i7 = 0; c26_i7 < 3; c26_i7++) {
c26_nuRef[c26_i7] = c26_nu_ref[c26_i7];
}
_SFD_EML_CALL(0U, chartInstance->c26_sfEvent, 15);
c26_x = c26_psi_ref;
c26_c = c26_x;
c26_b_x = c26_c;
c26_c = c26_b_x;
c26_c = muDoubleScalarCos(c26_c);
_SFD_EML_CALL(0U, chartInstance->c26_sfEvent, 16);
c26_c_x = c26_psi_ref;
c26_s = c26_c_x;
c26_d_x = c26_s;
c26_s = c26_d_x;
c26_s = muDoubleScalarSin(c26_s);
_SFD_EML_CALL(0U, chartInstance->c26_sfEvent, 17);
c26_R[0] = c26_c;
c26_R[3] = -c26_s;
c26_R[6] = 0.0;
c26_R[1] = c26_s;
c26_R[4] = c26_c;
c26_R[7] = 0.0;
c26_i8 = 0;
for (c26_i9 = 0; c26_i9 < 3; c26_i9++) {
c26_R[c26_i8 + 2] = c26_dv1[c26_i9];
c26_i8 += 3;
}
_SFD_EML_CALL(0U, chartInstance->c26_sfEvent, 21);
for (c26_i10 = 0; c26_i10 < 9; c26_i10++) {
c26_a[c26_i10] = c26_R[c26_i10];
}
for (c26_i11 = 0; c26_i11 < 3; c26_i11++) {
c26_b[c26_i11] = c26_nuRef[c26_i11];
}
c26_eml_scalar_eg(chartInstance);
c26_eml_scalar_eg(chartInstance);
for (c26_i12 = 0; c26_i12 < 3; c26_i12++) {
c26_eta_dot_ref[c26_i12] = 0.0;
}
for (c26_i13 = 0; c26_i13 < 3; c26_i13++) {
c26_eta_dot_ref[c26_i13] = 0.0;
}
for (c26_i14 = 0; c26_i14 < 3; c26_i14++) {
c26_C[c26_i14] = c26_eta_dot_ref[c26_i14];
}
for (c26_i15 = 0; c26_i15 < 3; c26_i15++) {
c26_eta_dot_ref[c26_i15] = c26_C[c26_i15];
}
for (c26_i16 = 0; c26_i16 < 3; c26_i16++) {
c26_C[c26_i16] = c26_eta_dot_ref[c26_i16];
}
for (c26_i17 = 0; c26_i17 < 3; c26_i17++) {
c26_eta_dot_ref[c26_i17] = c26_C[c26_i17];
}
for (c26_i18 = 0; c26_i18 < 3; c26_i18++) {
c26_eta_dot_ref[c26_i18] = 0.0;
c26_i19 = 0;
for (c26_i20 = 0; c26_i20 < 3; c26_i20++) {
c26_eta_dot_ref[c26_i18] += c26_a[c26_i19 + c26_i18] * c26_b[c26_i20];
c26_i19 += 3;
}
}
_SFD_EML_CALL(0U, chartInstance->c26_sfEvent, -21);
_SFD_SYMBOL_SCOPE_POP();
for (c26_i21 = 0; c26_i21 < 3; c26_i21++) {
(*c26_b_eta_dot_ref)[c26_i21] = c26_eta_dot_ref[c26_i21];
}
_SFD_CC_CALL(EXIT_OUT_OF_FUNCTION_TAG, 14U, chartInstance->c26_sfEvent);
}
static void sf_c1_Model_justmodel2(SFc1_Model_justmodel2InstanceStruct
*chartInstance)
{
int32_T c1_i2;
real_T c1_hoistedGlobal;
real_T c1_time;
uint32_T c1_debug_family_var_map[5];
real_T c1_surge;
real_T c1_nargin = 1.0;
real_T c1_nargout = 1.0;
real_T c1_Force[3];
int32_T c1_i3;
real_T c1_b;
real_T c1_y;
real_T c1_b_b;
real_T c1_b_y;
real_T c1_x;
real_T c1_b_x;
real_T c1_c_b;
real_T c1_c_y;
real_T c1_d_b;
real_T c1_d_y;
real_T c1_c_x;
real_T c1_d_x;
real_T c1_e_b;
real_T c1_e_y;
real_T c1_e_x;
real_T c1_f_x;
real_T c1_f_y;
real_T c1_varargin_1;
real_T c1_varargin_2;
real_T c1_g_x;
real_T c1_h_x;
real_T c1_xk;
real_T c1_i_x;
int32_T c1_i4;
real_T *c1_b_time;
real_T (*c1_b_Force)[3];
c1_b_Force = (real_T (*)[3])ssGetOutputPortSignal(chartInstance->S, 1);
c1_b_time = (real_T *)ssGetInputPortSignal(chartInstance->S, 0);
_sfTime_ = (real_T)ssGetT(chartInstance->S);
_SFD_CC_CALL(CHART_ENTER_SFUNCTION_TAG, 0U, chartInstance->c1_sfEvent);
_SFD_DATA_RANGE_CHECK(*c1_b_time, 0U);
for (c1_i2 = 0; c1_i2 < 3; c1_i2++) {
_SFD_DATA_RANGE_CHECK((*c1_b_Force)[c1_i2], 1U);
}
chartInstance->c1_sfEvent = CALL_EVENT;
_SFD_CC_CALL(CHART_ENTER_DURING_FUNCTION_TAG, 0U, chartInstance->c1_sfEvent);
c1_hoistedGlobal = *c1_b_time;
c1_time = c1_hoistedGlobal;
_SFD_SYMBOL_SCOPE_PUSH_EML(0U, 5U, 5U, c1_debug_family_names,
c1_debug_family_var_map);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c1_surge, 0U, c1_b_sf_marshallOut,
c1_b_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c1_nargin, 1U, c1_b_sf_marshallOut,
c1_b_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c1_nargout, 2U, c1_b_sf_marshallOut,
c1_b_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML(&c1_time, 3U, c1_b_sf_marshallOut);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(c1_Force, 4U, c1_sf_marshallOut,
c1_sf_marshallIn);
CV_EML_FCN(0, 0);
_SFD_EML_CALL(0U, chartInstance->c1_sfEvent, 3);
for (c1_i3 = 0; c1_i3 < 3; c1_i3++) {
c1_Force[c1_i3] = 0.0;
}
_SFD_EML_CALL(0U, chartInstance->c1_sfEvent, 5);
c1_b = c1_time;
c1_y = 0.066666666666666666 * c1_b;
c1_b_b = c1_time;
c1_b_y = 0.5 * c1_b_b;
c1_x = c1_b_y;
c1_b_x = c1_x;
c1_b_x = muDoubleScalarCos(c1_b_x);
c1_c_b = c1_b_x;
c1_c_y = 10.0 * c1_c_b;
c1_d_b = c1_time;
c1_d_y = 0.3 * c1_d_b;
c1_c_x = c1_d_y + 0.32;
c1_d_x = c1_c_x;
c1_d_x = muDoubleScalarSin(c1_d_x);
c1_e_b = c1_d_x;
c1_e_y = 14.0 * c1_e_b;
c1_e_x = c1_c_y + c1_e_y;
c1_f_x = c1_e_x;
c1_f_y = muDoubleScalarAbs(c1_f_x);
c1_varargin_1 = c1_y + c1_f_y;
c1_varargin_2 = c1_varargin_1;
c1_g_x = c1_varargin_2;
c1_h_x = c1_g_x;
c1_eml_scalar_eg(chartInstance);
c1_xk = c1_h_x;
c1_i_x = c1_xk;
c1_eml_scalar_eg(chartInstance);
c1_surge = muDoubleScalarMin(c1_i_x, 40.0);
_SFD_EML_CALL(0U, chartInstance->c1_sfEvent, 7);
c1_Force[0] = c1_surge;
_SFD_EML_CALL(0U, chartInstance->c1_sfEvent, -7);
_SFD_SYMBOL_SCOPE_POP();
for (c1_i4 = 0; c1_i4 < 3; c1_i4++) {
(*c1_b_Force)[c1_i4] = c1_Force[c1_i4];
}
_SFD_CC_CALL(EXIT_OUT_OF_FUNCTION_TAG, 0U, chartInstance->c1_sfEvent);
_SFD_CHECK_FOR_STATE_INCONSISTENCY(_Model_justmodel2MachineNumber_,
chartInstance->chartNumber, chartInstance->instanceNumber);
}
/* Function Definitions */
void clcPMP_olyHyb_tmp(const emlrtStack *sp, real_T engKinPre, real_T engKinAct,
real_T gea, real_T slp, real_T batEng, real_T psiBatEng, real_T psiTim, real_T
batPwrAux, real_T batEngStp, real_T wayStp, const struct0_T *par, real_T
*cosHamMin, real_T *batFrcOut, real_T *fulFrcOut)
{
real_T mtmp;
real_T vehVel;
real_T b_engKinPre[2];
real_T crsSpdVec[2];
int32_T i18;
int32_T k;
boolean_T y;
boolean_T exitg3;
boolean_T exitg2;
real_T crsSpd;
real_T whlTrq;
real_T crsTrq;
real_T iceTrqMax;
real_T iceTrqMin;
real_T b_par[100];
real_T emoTrqMaxPos;
real_T emoTrqMinPos;
real_T emoTrqMax;
real_T emoTrqMin;
real_T batPwrMax;
real_T batPwrMin;
real_T batOcv;
real_T batEngDltMin;
real_T batEngDltMax;
real_T batEngDltMinInx;
real_T batEngDltMaxInx;
real_T batEngDlt;
real_T fulFrc;
real_T batFrc;
real_T b_batFrc;
real_T batPwr;
real_T emoTrq;
real_T iceTrq;
real_T fulPwr;
int32_T ixstart;
int32_T itmp;
int32_T ix;
boolean_T exitg1;
emlrtStack st;
emlrtStack b_st;
st.prev = sp;
st.tls = sp->tls;
b_st.prev = &st;
b_st.tls = st.tls;
/* CLCPMP Minimizing Hamiltonian: Co-States for soc and time */
/* Erstellungsdatum der ersten Version 19.08.2015 - Stephan Uebel */
/* */
/* Batterieleistungsgrenzen hinzugefügt am 13.12.2015 */
/* ^^added battery power limit */
/* */
/* Massenaufschlag durch Trägheitsmoment herausgenommen */
/* ^^Mass increment removed by inertia */
/* */
/* % Inputdefinition */
/* */
/* engKinPre - Double(1,1) - kinetische Energie am Intervallanfang in J */
/* ^^ kinetic energy at start of interval (J) */
/* engKinAct - Double(1,1) - kinetische Energie am Intervallende in J */
/* ^^ kinetic energe at end of interval (J) */
/* gea - Double(1,1) - Gang */
/* ^^ gear */
/* slp - Double(1,1) - Steigung in rad */
/* ^^ slope in radians */
/* iceFlg - Boolean(1,1) - Flag für Motorzustand */
/* ^^ flag for motor condition */
/* batEng - Double(1,1) - Batterieenergie in J */
/* ^^ battery energy (J) */
/* psibatEng - Double(1,1) - Costate für Batterieenergie ohne Einheit */
/* ^^ costate for battery energy w/o unity */
/* psiTim - Double(1,1) - Costate für die Zeit ohne Einheit */
/* ^^ costate for time without unity */
/* batPwrAux - Double(1,1) - elektr. Nebenverbraucherleistung in W */
/* ^^ electric auxiliary power consumed (W) */
/* batEngStp - Double(1,1) - Drehmomentschritt */
/* ^^ torque step <- no, it's a battery step */
/* wayStp - Double(1,1) - Intervallschrittweite in m */
/* ^^ interval step distance (m) */
/* par - Struct(1,1) - Modelldaten */
/* ^^ model data */
/* % Initialisieren der Ausgabe der Funktion */
/* initializing function output */
/* Ausgabewert des Minimums der Hamiltonfunktion */
/* output for minimizing the hamiltonian */
*cosHamMin = rtInf;
/* Batterieladungsänderung im Wegschritt beir minimaler Hamiltonfunktion */
/* battery change in path_idx step with the minial hamiltonian */
*batFrcOut = rtInf;
/* Kraftstoffkraftänderung im Wegschritt bei minimaler Hamiltonfunktion */
/* fuel change in path_idx step with the minimal hamiltonian */
*fulFrcOut = 0.0;
/* % Initialisieren der persistent Größen */
/* initialize the persistance variables */
/* Diese werden die nur einmal für die Funktion berechnet */
/* only calculated once for the function */
if (!crsSpdHybMax_not_empty) {
/* maximale Drehzahl Elektrommotor */
/* maximum electric motor rotational speed */
/* maximale Drehzahl der Kurbelwelle */
/* maximum crankshaft rotational speed */
crsSpdHybMax = muDoubleScalarMin(par->iceSpdMgd[14850], par->emoSpdMgd[14850]);
crsSpdHybMax_not_empty = true;
/* minimale Drehzahl der Kurbelwelle */
/* minimum crankshaft rotational speed */
crsSpdHybMin = par->iceSpdMgd[0];
}
/* % Initialisieren der allgemein benötigten Kenngrößen */
/* initializing the commonly required parameters */
/* mittlere kinetische Energie im Wegschritt berechnen */
/* define the average kinetic energy at path_idx step - is just previous KE */
/* mittlere Geschwindigkeit im Wegschritt berechnen */
/* define the average speed at path_idx step */
mtmp = 2.0 * engKinPre / par->vehMas;
st.site = &g_emlrtRSI;
if (mtmp < 0.0) {
b_st.site = &h_emlrtRSI;
eml_error(&b_st);
}
vehVel = muDoubleScalarSqrt(mtmp);
/* % vorzeitiger Funktionsabbruch? */
/* premature function termination? */
/* Drehzahl der Kurbelwelle und Grenzen */
/* crankshaft speed and limits */
/* Aus den kinetischen Energien des Fahrzeugs wird über die Raddrehzahl */
/* und die übersetzung vom Getriebe die Kurbelwellendrehzahl berechnet. */
/* Zeilenrichtung entspricht den Gängen. (Zeilenvektor) */
/* from the vehicle's kinetic energy, the crankshaft speed is calculated */
/* by the speed and gearbox translation. Line direction corresponding to */
/* the aisles (row rector). EQUATION 1 */
b_engKinPre[0] = engKinPre;
b_engKinPre[1] = engKinAct;
for (i18 = 0; i18 < 2; i18++) {
crsSpdVec[i18] = 2.0 * b_engKinPre[i18] / par->vehMas;
}
st.site = &f_emlrtRSI;
for (k = 0; k < 2; k++) {
if (crsSpdVec[k] < 0.0) {
b_st.site = &h_emlrtRSI;
eml_error(&b_st);
}
}
for (k = 0; k < 2; k++) {
crsSpdVec[k] = muDoubleScalarSqrt(crsSpdVec[k]);
}
i18 = par->geaRat->size[1];
k = (int32_T)gea;
emlrtDynamicBoundsCheckR2012b(k, 1, i18, &mb_emlrtBCI, sp);
mtmp = par->geaRat->data[(int32_T)gea - 1];
for (i18 = 0; i18 < 2; i18++) {
crsSpdVec[i18] = mtmp * crsSpdVec[i18] / par->whlDrr;
}
/* Abbruch, wenn die Drehzahlen der Kurbelwelle zu hoch im hybridischen */
/* Modus */
/* stop if the crankshaft rotatoinal speed is too high in hybrid mode */
y = false;
k = 0;
exitg3 = false;
while ((!exitg3) && (k < 2)) {
if (!!(crsSpdVec[k] > crsSpdHybMax)) {
y = true;
exitg3 = true;
} else {
k++;
}
}
if (y) {
} else {
/* Falls die Drehzahl des Verbrennungsmotors niedriger als die */
/* Leerlaufdrehzahl ist, */
/* stop if crankhaft rotional speed is lower than the idling speed */
y = false;
k = 0;
exitg2 = false;
while ((!exitg2) && (k < 2)) {
if (!!(crsSpdVec[k] < crsSpdHybMin)) {
y = true;
exitg2 = true;
} else {
k++;
}
}
if (y) {
} else {
/* Prüfen, ob die Drehzahlgrenze des Elektromotors eingehalten wird */
/* check if electric motor speed limit is maintained */
/* mittlere Kurbelwellendrehzahlen berechnen */
/* calculate average crankshaft rotational speed */
/* - really just selecting the previous path_idx KE crankshaft speed */
crsSpd = crsSpdVec[0];
/* % Längsdynamik berechnen */
/* calculate longitundinal dynamics */
/* Es wird eine konstante Beschleunigung angenommen, die im Wegschritt */
/* wayStp das Fahrzeug von velPre auf velAct beschleunigt. */
/* constant acceleration assumed when transitioning from velPre to velAct */
/* for the selected wayStp path_idx step distance */
/* Berechnen der konstanten Beschleunigung */
/* calculate the constant acceleration */
/* Aus der mittleren kinetischen Energie im Intervall, der mittleren */
/* Steigung und dem Gang lässt sich über die Fahrwiderstandsgleichung */
/* die nötige Fahrwiderstandskraft berechnen, die aufgebracht werden */
/* muss, um diese zu realisieren. */
/* from the (avg) kinetic energy in the interval, the (avg) slope and */
/* transition can calculate the necessary traction force on the driving */
/* resistance equation (PART OF EQUATION 5) */
/* Steigungskraft aus der mittleren Steigung berechnen (Skalar) */
/* gradiant force from the calculated (average) gradient */
/* Rollreibungskraft berechnen (Skalar) */
/* calculated rolling friction force - not included in EQ 5??? */
/* Luftwiderstandskraft berechnen (2*c_a/m * E_kin) (Skalar) */
/* calculated air resistance force */
/* % Berechnung der minimalen kosten der Hamiltonfunktion */
/* Calculating the minimum cost of the Hamiltonian */
/* % Berechnen der Kraft am Rad für Antriebsstrangmodus */
/* calculate the force on the wheel for the drivetrain mode */
/* % dynamische Fahrzeugmasse bei Fahrzeugmotor an berechnen. Das */
/* % heißt es werden Trägheitsmoment von Verbrennungsmotor, */
/* % Elektromotor und Rädern mit einbezogen. */
/* calculate dynamic vehicle mass with the vehicle engine (with the moment */
/* of intertia of the ICE, electric motor, and wheels) */
/* vehMasDyn = (par.iceMoi_geaRat(gea) +... */
/* par.emoGeaMoi_geaRat(gea) + par.whlMoi)/par.whlDrr^2 ... */
/* + par.vehMas; */
/* Radkraft berechnen (Beschleunigungskraft + Steigungskraft + */
/* Rollwiderstandskraft + Luftwiderstandskraft) */
/* caluclating wheel forces (accerlation force + gradient force + rolling */
/* resistance + air resistance) EQUATION 5 */
/* % Getriebeübersetzung und -verlust */
/* gear ratio and loss */
/* Das Drehmoment des Rades ergibt sich über den Radhalbmesser aus */
/* der Fahrwiderstandskraft. */
/* the weel torque is obtained from the wheel radius of the rolling */
/* resistance force (torque = force * distance (in this case, radius) */
whlTrq = ((((engKinAct - engKinPre) / (par->vehMas * wayStp) * par->vehMas
+ par->vehMas * 9.81 * muDoubleScalarSin(slp)) +
par->whlRolResCof * par->vehMas * 9.81 * muDoubleScalarCos(slp))
+ 2.0 * par->drgCof / par->vehMas * engKinPre) * par->whlDrr;
/* Berechnung des Kurbelwellenmoments */
/* Hier muss unterschieden werden, ob das Radmoment positiv oder */
/* negativ ist, da nur ein einfacher Wirkungsgrad für das Getriebe */
/* genutzt wird */
/* it's important to determine sign of crankshaft torque (positive or */
/* negative), since only a simple efficiency is used for the transmission */
/* PART OF EQ4 <- perhaps reversed? not too sure */
if (whlTrq < 0.0) {
i18 = par->geaRat->size[1];
k = (int32_T)gea;
emlrtDynamicBoundsCheckR2012b(k, 1, i18, &nb_emlrtBCI, sp);
crsTrq = whlTrq / par->geaRat->data[(int32_T)gea - 1] * par->geaEfy;
} else {
i18 = par->geaRat->size[1];
k = (int32_T)gea;
emlrtDynamicBoundsCheckR2012b(k, 1, i18, &ob_emlrtBCI, sp);
crsTrq = whlTrq / par->geaRat->data[(int32_T)gea - 1] / par->geaEfy;
}
/* % Verbrennungsmotor */
/* internal combustion engine */
/* maximales Moment des Verbrennungsmotors berechnen */
/* calculate max torque of the engine (quadratic based on rotation speed) */
iceTrqMax = (par->iceTrqMaxCof[0] * (crsSpdVec[0] * crsSpdVec[0]) +
par->iceTrqMaxCof[1] * crsSpdVec[0]) + par->iceTrqMaxCof[2];
/* minimales Moment des Verbrennungsmotors berechnen */
/* calculating mimimum ICE moment */
iceTrqMin = (par->iceTrqMinCof[0] * (crsSpdVec[0] * crsSpdVec[0]) +
par->iceTrqMinCof[1] * crsSpdVec[0]) + par->iceTrqMinCof[2];
/* % Elektromotor */
/* electric motor */
/* maximales Moment, dass die E-Maschine liefern kann */
/* max torque that the electric motor can provide - from interpolation */
/* emoTrqMaxPos = ... */
/* lininterp1(par.emoSpdMgd(1,:)',par.emoTrqMax_emoSpd,crsSpd); */
for (i18 = 0; i18 < 100; i18++) {
b_par[i18] = par->emoSpdMgd[150 * i18];
}
emoTrqMaxPos = interp1q(b_par, par->emoTrqMax_emoSpd, crsSpdVec[0]);
/* Die gültigen Kurbelwellenmomente müssen kleiner sein als das */
/* Gesamtmoment von E-Motor und Verbrennungsmotor */
/* The valid crankshaft moments must be less than the total moment of the */
/* electric motor and the ICE.Otherwise, leave the function */
if (crsTrq > iceTrqMax + emoTrqMaxPos) {
} else {
/* % %% Optimaler Momentensplit - Minimierung der Hamiltonfunktion */
/* optimum torque split - minimizing the Hamiltonian */
/* Die Vorgehensweise ist ähnlich wie bei der ECMS. Es wird ein Vektor der */
/* möglichen Batterieenergieänderungen aufgestellt. Aus diesen lässt sich */
/* eine Batterieklemmleistung berechnen. Aus der über das */
/* Kurbelwellenmoment, ein Elektromotormoment berechnet werden kann. */
/* Über das geforderte Kurbelwellenmoment, kann für jedes Moment des */
/* Elektromotors ein Moment des Verbrennungsmotors gefunden werden. Für */
/* jedes Momentenpaar kann die Hamiltonfunktion berechnet werden. */
/* Ausgegeben wird der minimale Wert der Hamiltonfunktion und die */
/* durch das dabei verwendete Elektromotormoment verursachte */
/* Batterieladungsänderung. */
/* The procedure is similar to ECMS. It's based on a vector of possible */
/* battery energy changes, from which a battery terminal power can be */
/* calculated. */
/* From the crankshaft torque, an electric motor torque can be */
/* calculated, and an engine torque can be found for every electric motor */
/* moment. */
/* For every moment-pair the Hamiltonian can be calculated. It */
/* outputs the minimum Hamilotnian value and the battery charge change */
/* caused by the electric motor torque used. */
/* % Elektromotor - Aufstellen des Batterienergievektors */
/* electric motor - positioning the battery energy vectors */
if (batEngStp > 0.0) {
/* Skalar - änderung der minimalen Batterieenergieänderung */
/* Skalar - änderung der maximalen Batterieenergieänderung */
/* FUNCTION CALL */
/* Skalar - Wegschrittweite */
/* Skalar - mittlere Geschwindigkeit im Intervall */
/* Skalar - Nebenverbraucherlast */
/* Skalar - Batterieenergie */
/* struct - Fahrzeugparameter */
/* Skalar - crankshaft rotational speed */
/* Skalar - crankshaft torque */
/* Skalar - min ICE torque allowed */
/* Skalar - max ICE torque allowed */
/* Skalar - max EM torque possible */
st.site = &e_emlrtRSI;
/* Skalar - änderung der minimalen Batterieenergieänderung */
/* Skalar - änderung der maximalen Batterieenergieänderung */
/* Skalar - Wegschrittweite */
/* Skalar - Geschwindigkeit im Intervall */
/* Skalar - Nebenverbraucherlast */
/* Skalar - Batterieenergie */
/* struct - Fahrzeugparameter */
/* Skalar - crankshaft rotational speed */
/* Skalar - crankshaft torque */
/* Skalar - min ICE torque allowed */
/* Skalar - max ICE torque */
/* Skalar - max EM torque possible */
/* BatEngDltClc Calculates the change in battery energy */
/* */
/* Erstellungsdatum der ersten Version 17.11.2015 - Stephan Uebel */
/* Berechnung der Verluste des Elektromotors bei voller rein elektrischer */
/* Fahrt (voller Lastpunktabsenkung) und bei voller Lastpunktanhebung */
/* Calculations of loss of electric motor at purely full electric */
/* Driving (full load point lowering) and at full load point raising */
/* */
/* Version vom 17.02.2016: Keine Einbeziehung von Rotationsmassen */
/* ^^ No inclusion of rotational masses */
/* */
/* Version vom 25.05.2016: Zero-Order-Hold (keine mittlere Geschwindigkeit) */
/* ^^ Zero-Order-Hold (no average velocities) */
/* % Initialisieren der Ausgabe der Funktion */
/* initializing the function output (delta battery_energy min and max) */
/* % Elektromotor */
/* minimales Moment, dass die E-Maschine liefern kann */
/* minimum moment that the EM can provide (max is an input to function) */
/* emoTrqMinPos = ... */
/* lininterp1(par.emoSpdMgd(1,:)',par.emoTrqMin_emoSpd,crsSpd); */
for (i18 = 0; i18 < 100; i18++) {
b_par[i18] = par->emoSpdMgd[150 * i18];
}
emoTrqMinPos = interp1q(b_par, par->emoTrqMin_emoSpd, crsSpdVec[0]);
/* % Verbrennungsmotor berechnen */
/* Durch EM zu lieferndes Kurbelwellenmoment */
/* crankshaft torque to be delivered by the electric motor (min and max) */
emoTrqMax = crsTrq - iceTrqMin;
emoTrqMin = crsTrq - iceTrqMax;
/* % Elektromotor berechnen */
/* calculate the electric motor */
if (emoTrqMaxPos < emoTrqMax) {
/* Moment des Elektromotors bei maximaler Entladung der Batterie */
/* EM torque at max battery discharge */
emoTrqMax = emoTrqMaxPos;
}
if (emoTrqMaxPos < emoTrqMin) {
/* Moment des Elektromotors bei minimaler Entladung der Batterie */
/* EM torque at min battery discharge */
emoTrqMin = emoTrqMaxPos;
}
emoTrqMax = muDoubleScalarMax(emoTrqMinPos, emoTrqMax);
emoTrqMin = muDoubleScalarMax(emoTrqMinPos, emoTrqMin);
/* % Berechnung der änderung der Batterieladung */
/* calculating the change in battery charge */
/* Interpolation der benötigten Batterieklemmleistung für das */
/* EM-Moment. Stellen die nicht definiert sind, werden mit inf */
/* ausgegeben. Positive Werte entsprechen entladen der Batterie. */
/* interpolating the required battery terminal power for the EM torque. */
/* Assign undefined values to inf. Positive values coresspond with battery */
/* discharge. */
/* batPwrMax = lininterp2(par.emoSpdMgd(1,:),par.emoTrqMgd(:,1),... */
/* par.emoPwr_emoSpd_emoTrq',crsSpd,emoTrqMax) + batPwrAux; */
/* */
/* batPwrMin = lininterp2(par.emoSpdMgd(1,:),par.emoTrqMgd(:,1),... */
/* par.emoPwr_emoSpd_emoTrq',crsSpd,emoTrqMin) + batPwrAux; */
b_st.site = &i_emlrtRSI;
batPwrMax = codegen_interp2(&b_st, par->emoSpdMgd, par->emoTrqMgd,
par->emoPwr_emoSpd_emoTrq, crsSpdVec[0], emoTrqMax) + batPwrAux;
b_st.site = &j_emlrtRSI;
batPwrMin = codegen_interp2(&b_st, par->emoSpdMgd, par->emoTrqMgd,
par->emoPwr_emoSpd_emoTrq, crsSpdVec[0], emoTrqMin) + batPwrAux;
/* überprüfen, ob Batterieleistung möglich */
/* make sure that current battery max power is not above bat max bounds */
if (batPwrMax > par->batPwrMax) {
batPwrMax = par->batPwrMax;
}
/* überprüfen, ob Batterieleistung möglich */
/* make sure that current battery min power is not below bat min bounds */
if (batPwrMin > par->batPwrMax) {
batPwrMin = par->batPwrMax;
}
/* Es kann vorkommen, dass mehr Leistung gespeist werden soll, als */
/* möglich ist. */
/* double check that the max and min still remain within the other bounds */
if (batPwrMax < par->batPwrMin) {
batPwrMax = par->batPwrMin;
}
if (batPwrMin < par->batPwrMin) {
batPwrMin = par->batPwrMin;
}
/* Batteriespannung aus Kennkurve berechnen */
/* calculating battery voltage of characteristic curve - eq?-------------- */
batOcv = batEng * par->batOcvCof_batEng[0] + par->batOcvCof_batEng[1];
/* FUNCTION CALL - min delta bat.energy */
/* Skalar - Batterieklemmleistung */
/* Skalar - mittlere Geschwindigkeit im Intervall */
/* Skalar - Entladewiderstand */
/* Skalar - Ladewiderstand */
/* Skalar - battery open-circuit voltage */
batEngDltMin = batFrcClc_tmp(batPwrMax, vehVel, par->batRstDch,
par->batRstChr, batOcv) * wayStp;
/* <-multiply by delta_S */
/* FUNCTION CALL - max delta bat.energy */
/* Skalar - Batterieklemmleistung */
/* Skalar - mittlere Geschwindigkeit im Intervall */
/* Skalar - Entladewiderstand */
/* Skalar - Ladewiderstand */
/* Skalar - battery open-circuit voltage */
batEngDltMax = batFrcClc_tmp(batPwrMin, vehVel, par->batRstDch,
par->batRstChr, batOcv) * wayStp;
/* Es werden 2 Fälle unterschieden: */
/* there are 2 different cases */
if ((batEngDltMin > 0.0) && (batEngDltMax > 0.0)) {
/* %% konventionelles Bremsen + Rekuperieren */
/* conventional brakes + recuperation */
/* */
/* set change in energy to discretized integer values w/ ceil and */
/* floor. This also helps for easy looping */
/* Konventionelles Bremsen wird ebenfalls untersucht. */
/* Hier liegt eventuell noch Beschleunigungspotential, da diese */
/* Zustandswechsel u.U. ausgeschlossen werden können. */
/* NOTE: u.U. = unter Ümständen = circumstances permitting */
/* convetional breaks also investigated. An accelerating potential */
/* is still possible, as these states can be excluded */
/* (circumstances permitting) <- ??? not sure what above means */
/* */
/* so if both min and max changes in battery energy are */
/* increasing, then set the delta min to zero */
batEngDltMinInx = 0.0;
batEngDltMaxInx = muDoubleScalarFloor(batEngDltMax / batEngStp);
} else {
batEngDltMinInx = muDoubleScalarCeil(batEngDltMin / batEngStp);
batEngDltMaxInx = muDoubleScalarFloor(batEngDltMax / batEngStp);
}
} else {
batEngDltMinInx = 0.0;
batEngDltMaxInx = 0.0;
}
/* you got a larger min chnage and a max change, you're out of bounds. Leave */
/* the function */
if (batEngDltMaxInx < batEngDltMinInx) {
} else {
/* % Schleife über alle Elektromotormomente */
/* now loop through all the electric-motor torques */
i18 = (int32_T)(batEngDltMaxInx + (1.0 - batEngDltMinInx));
emlrtForLoopVectorCheckR2012b(batEngDltMinInx, 1.0, batEngDltMaxInx,
mxDOUBLE_CLASS, i18, &o_emlrtRTEI, sp);
k = 0;
while (k <= i18 - 1) {
batEngDlt = (batEngDltMinInx + (real_T)k) * batEngStp;
/* open circuit voltage over each iteration */
batOcv = (batEng + batEngDlt) * par->batOcvCof_batEng[0] +
par->batOcvCof_batEng[1];
/* Skalar für die Batterieleistung in W */
/* Skalar Krafstoffkraft in N */
/* FUNCTION CALL */
/* Skalar für die Wegschrittweite in m, */
/* Skalar - vehicular velocity */
/* Nebenverbraucherlast */
/* Skalar - battery open circuit voltage */
/* Skalar - Batterieenergie�nderung, */
/* Skalar - crankshaft speed at given path_idx */
/* Skalar - crankshaft torque at given path_idx */
/* Skalar - min ICE torque allowed */
/* Skalar - max ICE torque */
/* struct der Fahrzeugparameter */
st.site = &d_emlrtRSI;
/* Skalar für die Batterieleistung */
/* Skalar Kraftstoffkraft */
/* Skalar für die Wegschrittweite in m */
/* vehicular velocity */
/* Nebenverbraucherlast */
/* Skalar - battery open circuit voltage */
/* Skalar - Batterieenergieänderung */
/* Skalar - crankshaft speed at given path_idx */
/* Skalar - crankshaft torque at given path_idx */
/* Skalar - min ICE torque allowed */
/* Skalar - max ICE torque */
/* struct der Fahrzeugparameter */
/* */
/* FULENGCLC Calculating fuel consumption */
/* Erstellungsdatum der ersten Version 04.09.2015 - Stephan Uebel */
/* */
/* Diese Funktion berechnet den Kraftstoffverbrauch für einen gegebenen */
/* Wegschritt der kinetischen Energie, der Batterieenergie und des */
/* Antriebsstrangzustands über dem Weg. */
/* this function calculates fuel consumption for a given route step of */
/* KE, the battery energy, and drivetrain state of the current path_idx */
/* */
/* Version vom 17.02.2016 : Rotationsmassen vernachlässigt */
/* ^^ neglected rotatary masses */
/* */
/* Version vom 25.05.2016: Zero-Order-Hold (keine mittlere Geschwindigkeit) */
/* */
/* version from 1.06.2016 - removed crsTrq calulations - they are already */
/* done in parent function (clcPMP_olHyb_tmp) and do not change w/ each */
/* iteration, making the caluclation here unnecessary */
/* % Initialisieren der Ausgabe der Funktion */
/* initializing function output */
/* Skalar - electric battery power (W) */
fulFrc = rtInf;
/* Skalar - fuel force intialization (N) */
/* % Batterie */
/* Batterieenergieänderung über dem Weg (Batteriekraft) */
/* Change in battery energy over the path_idx way (ie battery power) */
batFrc = batEngDlt / wayStp;
/* Fallunterscheidung, ob Batterie geladen oder entladen wird */
/* Case analysis - check if battery is charging or discharging. Set */
/* resistance accordingly */
/* elektrische Leistung des Elektromotors */
/* electric power from electric motor - DERIVATION? dunno */
/* innere Batterieleistung / internal batt power */
/* dissipat. Leist. / dissipated */
if (batFrc < 0.0) {
b_batFrc = par->batRstDch;
} else {
b_batFrc = par->batRstChr;
}
batPwr = (-batFrc * vehVel - batFrc * batFrc * (vehVel * vehVel) /
(batOcv * batOcv) * b_batFrc) - batPwrAux;
/* Nebenverbrauchlast / auxiliary power */
/* % Elektromotor */
/* Ermitteln des Kurbelwellenmoments durch EM aus Batterieleistung */
/* determine crankshaft torque cauesd by EM's battery power */
/* switching out codegen_interp2 for lininterp2-just might work! */
/* */
b_st.site = &k_emlrtRSI;
emoTrq = codegen_interp2(&b_st, par->emoSpdMgd, par->emoPwrMgd,
par->emoTrq_emoSpd_emoPwr, crsSpd, batPwr);
/* emoTrq = lininterp2(par.emoSpdMgd(1,:), par.emoPwrMgd(:,1),... */
/* par.emoTrq_emoSpd_emoPwr',crsSpd,emoPwrEle); */
if (muDoubleScalarIsInf(emoTrq)) {
} else {
/* Grenzen des Elektromotors müssen hier nicht überprüft werden, da die */
/* Ladungsdeltas schon so gewählt wurden, dass das maximale Motormoment */
/* nicht überstiegen wird. */
/* Electric motor limits need not be checked here, since the charge deltas */
/* have been chosen so that the max torque is not exceeded. */
/* % Verbrennungsmotor */
/* Ermitteln des Kurbelwellenmoments durch den Verbrennungsmotor */
/* Determining the crankshaft moment from the ICE */
iceTrq = crsTrq - emoTrq;
/* Wenn das Verbrennungsmotormoment kleiner als das minimale */
/* Moment ist, ist dieser in der Schubabschaltung. Das */
/* verbleibende Moment liefern die Bremsen */
/* If engine torque is less than the min torque, fuel is cut. The */
/* remaining torque is deliver the brakes. */
if (iceTrq < iceTrqMin) {
fulPwr = 0.0;
} else if (iceTrq > iceTrqMax) {
fulPwr = rtInf;
} else {
/* replacing another coden_interp2 */
b_st.site = &l_emlrtRSI;
fulPwr = codegen_interp2(&b_st, par->iceSpdMgd, par->iceTrqMgd,
par->iceFulPwr_iceSpd_iceTrq, crsSpd, iceTrq);
/* fulPwr = lininterp2(par.iceSpdMgd(1,:), par.iceTrqMgd(:,1), ... */
/* par.iceFulPwr_iceSpd_iceTrq', crsSpd, iceTrq); */
}
/* Berechnen der Kraftstoffkraft */
/* calculate fuel force */
fulFrc = fulPwr / vehVel;
/* Berechnen der Kraftstoffvolumenänderung */
/* caluclate change in fuel volume - energy, no? */
/* % Ende der Funktion */
}
/* FUNCTION CALL */
/* Skalar - Batterieklemmleistung */
/* Skalar - mittlere Geschwindigkeit im Intervall */
/* Skalar - Entladewiderstand */
/* Skalar - Ladewiderstand */
/* Skalar - battery open circuit voltage */
batFrc = batFrcClc_tmp(batPwr, vehVel, par->batRstDch,
par->batRstChr, batOcv);
/* %% Hamiltonfunktion bestimmen */
/* determine the hamiltonian */
/* Eq (29b) */
crsSpdVec[0] = (fulFrc + psiBatEng * batFrc) + psiTim / vehVel;
ixstart = 1;
mtmp = crsSpdVec[0];
itmp = 1;
if (muDoubleScalarIsNaN(crsSpdVec[0])) {
ix = 2;
exitg1 = false;
while ((!exitg1) && (ix < 3)) {
ixstart = 2;
if (!muDoubleScalarIsNaN(*cosHamMin)) {
mtmp = *cosHamMin;
itmp = 2;
exitg1 = true;
} else {
ix = 3;
}
}
}
if ((ixstart < 2) && (*cosHamMin < mtmp)) {
mtmp = *cosHamMin;
itmp = 2;
}
*cosHamMin = mtmp;
/* Wenn der aktuelle Punkt besser ist, als der in cosHamMin */
/* gespeicherte Wert, werden die Ausgabegrößen neu beschrieben. */
/* if the current point is better than the stored cosHamMin value, */
/* then the output values are rewritten (selected prev. value is = 2) */
if (itmp == 1) {
*batFrcOut = batFrc;
*fulFrcOut = fulFrc;
}
k++;
if (*emlrtBreakCheckR2012bFlagVar != 0) {
emlrtBreakCheckR2012b(sp);
}
}
}
}
}
}
/* end of function */
}
static void sf_c3_canudas_goal3(SFc3_canudas_goal3InstanceStruct *chartInstance)
{
int32_T c3_i0;
int32_T c3_i1;
real_T c3_hoistedGlobal;
real_T c3_b_hoistedGlobal;
real_T c3_c_hoistedGlobal;
real_T c3_d_hoistedGlobal;
int32_T c3_i2;
real_T c3_z[2];
real_T c3_thetad;
real_T c3_r;
int32_T c3_i3;
real_T c3_qe[3];
real_T c3_k1;
real_T c3_k2;
uint32_T c3_debug_family_var_map[13];
real_T c3_b;
real_T c3_b1;
real_T c3_b2;
real_T c3_nargin = 6.0;
real_T c3_nargout = 2.0;
real_T c3_w;
real_T c3_v;
real_T c3_A;
real_T c3_B;
real_T c3_x;
real_T c3_y;
real_T c3_b_x;
real_T c3_b_y;
real_T c3_c_x;
real_T c3_d_x;
real_T c3_b_A;
real_T c3_b_B;
real_T c3_e_x;
real_T c3_c_y;
real_T c3_f_x;
real_T c3_d_y;
real_T c3_e_y;
real_T c3_a;
real_T c3_b_b;
real_T c3_f_y;
real_T c3_g_x;
real_T c3_h_x;
real_T c3_c_B;
real_T c3_g_y;
real_T c3_h_y;
real_T c3_i_y;
real_T c3_b_a;
real_T c3_c_b;
real_T c3_j_y;
real_T c3_c_A;
real_T c3_i_x;
real_T c3_j_x;
real_T c3_k_y;
real_T c3_d_B;
real_T c3_l_y;
real_T c3_m_y;
real_T c3_n_y;
real_T c3_c_a;
real_T c3_d_b;
real_T c3_o_y;
real_T c3_k_x;
real_T c3_l_x;
real_T c3_d_a;
real_T c3_p_y;
real_T c3_e_a;
real_T c3_e_b;
real_T c3_q_y;
real_T c3_f_a;
real_T c3_f_b;
real_T c3_r_y;
real_T c3_d_A;
real_T c3_e_B;
real_T c3_m_x;
real_T c3_s_y;
real_T c3_n_x;
real_T c3_t_y;
real_T c3_u_y;
real_T c3_o_x;
real_T c3_p_x;
real_T c3_g_a;
real_T c3_v_y;
real_T c3_h_a;
real_T c3_g_b;
real_T c3_w_y;
real_T c3_e_A;
real_T c3_f_B;
real_T c3_q_x;
real_T c3_x_y;
real_T c3_r_x;
real_T c3_y_y;
real_T c3_ab_y;
real_T c3_i_a;
real_T c3_h_b;
real_T c3_bb_y;
real_T c3_j_a;
real_T c3_i_b;
real_T c3_k_a;
real_T c3_j_b;
real_T c3_cb_y;
real_T c3_l_a;
real_T c3_k_b;
real_T c3_db_y;
real_T *c3_b_v;
real_T *c3_b_w;
real_T *c3_b_k2;
real_T *c3_b_k1;
real_T *c3_b_r;
real_T *c3_b_thetad;
real_T (*c3_b_qe)[3];
real_T (*c3_b_z)[2];
c3_b_k2 = (real_T *)ssGetInputPortSignal(chartInstance->S, 5);
c3_b_k1 = (real_T *)ssGetInputPortSignal(chartInstance->S, 4);
c3_b_qe = (real_T (*)[3])ssGetInputPortSignal(chartInstance->S, 3);
c3_b_v = (real_T *)ssGetOutputPortSignal(chartInstance->S, 2);
c3_b_r = (real_T *)ssGetInputPortSignal(chartInstance->S, 2);
c3_b_thetad = (real_T *)ssGetInputPortSignal(chartInstance->S, 1);
c3_b_z = (real_T (*)[2])ssGetInputPortSignal(chartInstance->S, 0);
c3_b_w = (real_T *)ssGetOutputPortSignal(chartInstance->S, 1);
_sfTime_ = (real_T)ssGetT(chartInstance->S);
_SFD_CC_CALL(CHART_ENTER_SFUNCTION_TAG, 2U, chartInstance->c3_sfEvent);
_SFD_DATA_RANGE_CHECK(*c3_b_w, 0U);
for (c3_i0 = 0; c3_i0 < 2; c3_i0++) {
_SFD_DATA_RANGE_CHECK((*c3_b_z)[c3_i0], 1U);
}
_SFD_DATA_RANGE_CHECK(*c3_b_thetad, 2U);
_SFD_DATA_RANGE_CHECK(*c3_b_r, 3U);
_SFD_DATA_RANGE_CHECK(*c3_b_v, 4U);
for (c3_i1 = 0; c3_i1 < 3; c3_i1++) {
_SFD_DATA_RANGE_CHECK((*c3_b_qe)[c3_i1], 5U);
}
_SFD_DATA_RANGE_CHECK(*c3_b_k1, 6U);
_SFD_DATA_RANGE_CHECK(*c3_b_k2, 7U);
chartInstance->c3_sfEvent = CALL_EVENT;
_SFD_CC_CALL(CHART_ENTER_DURING_FUNCTION_TAG, 2U, chartInstance->c3_sfEvent);
c3_hoistedGlobal = *c3_b_thetad;
c3_b_hoistedGlobal = *c3_b_r;
c3_c_hoistedGlobal = *c3_b_k1;
c3_d_hoistedGlobal = *c3_b_k2;
for (c3_i2 = 0; c3_i2 < 2; c3_i2++) {
c3_z[c3_i2] = (*c3_b_z)[c3_i2];
}
c3_thetad = c3_hoistedGlobal;
c3_r = c3_b_hoistedGlobal;
for (c3_i3 = 0; c3_i3 < 3; c3_i3++) {
c3_qe[c3_i3] = (*c3_b_qe)[c3_i3];
}
c3_k1 = c3_c_hoistedGlobal;
c3_k2 = c3_d_hoistedGlobal;
sf_debug_symbol_scope_push_eml(0U, 13U, 13U, c3_debug_family_names,
c3_debug_family_var_map);
sf_debug_symbol_scope_add_eml_importable(&c3_b, 0U, c3_sf_marshallOut,
c3_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c3_b1, 1U, c3_sf_marshallOut,
c3_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c3_b2, 2U, c3_sf_marshallOut,
c3_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c3_nargin, 3U, c3_sf_marshallOut,
c3_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c3_nargout, 4U, c3_sf_marshallOut,
c3_sf_marshallIn);
sf_debug_symbol_scope_add_eml(c3_z, 5U, c3_c_sf_marshallOut);
sf_debug_symbol_scope_add_eml(&c3_thetad, 6U, c3_sf_marshallOut);
sf_debug_symbol_scope_add_eml(&c3_r, 7U, c3_sf_marshallOut);
sf_debug_symbol_scope_add_eml(c3_qe, 8U, c3_b_sf_marshallOut);
sf_debug_symbol_scope_add_eml(&c3_k1, 9U, c3_sf_marshallOut);
sf_debug_symbol_scope_add_eml(&c3_k2, 10U, c3_sf_marshallOut);
sf_debug_symbol_scope_add_eml_importable(&c3_w, 11U, c3_sf_marshallOut,
c3_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c3_v, 12U, c3_sf_marshallOut,
c3_sf_marshallIn);
CV_EML_FCN(0, 0);
_SFD_EML_CALL(0U, chartInstance->c3_sfEvent, 4);
c3_A = c3_qe[1];
c3_B = c3_qe[0];
c3_x = c3_A;
c3_y = c3_B;
c3_b_x = c3_x;
c3_b_y = c3_y;
c3_b = c3_b_x / c3_b_y;
_SFD_EML_CALL(0U, chartInstance->c3_sfEvent, 5);
c3_c_x = c3_qe[2];
c3_d_x = c3_c_x;
c3_d_x = muDoubleScalarCos(c3_d_x);
c3_b_A = c3_thetad;
c3_b_B = c3_b;
c3_e_x = c3_b_A;
c3_c_y = c3_b_B;
c3_f_x = c3_e_x;
c3_d_y = c3_c_y;
c3_e_y = c3_f_x / c3_d_y;
c3_a = c3_d_x;
c3_b_b = c3_e_y - 1.0;
c3_f_y = c3_a * c3_b_b;
c3_g_x = c3_qe[2];
c3_h_x = c3_g_x;
c3_h_x = muDoubleScalarSin(c3_h_x);
c3_c_B = c3_mpower(chartInstance, c3_b);
c3_g_y = c3_c_B;
c3_h_y = c3_g_y;
c3_i_y = 1.0 / c3_h_y;
c3_b_a = c3_thetad;
c3_c_b = 1.0 - c3_i_y;
c3_j_y = c3_b_a * c3_c_b;
c3_c_A = c3_j_y;
c3_i_x = c3_c_A;
c3_j_x = c3_i_x;
c3_k_y = c3_j_x / 2.0;
c3_d_B = c3_b;
c3_l_y = c3_d_B;
c3_m_y = c3_l_y;
c3_n_y = 1.0 / c3_m_y;
c3_c_a = c3_h_x;
c3_d_b = c3_k_y + c3_n_y;
c3_o_y = c3_c_a * c3_d_b;
c3_b1 = c3_f_y + c3_o_y;
_SFD_EML_CALL(0U, chartInstance->c3_sfEvent, 6);
c3_k_x = c3_qe[2];
c3_l_x = c3_k_x;
c3_l_x = muDoubleScalarCos(c3_l_x);
c3_d_a = c3_l_x;
c3_p_y = c3_d_a * 2.0;
c3_e_a = c3_p_y;
c3_e_b = c3_b;
c3_q_y = c3_e_a * c3_e_b;
c3_f_a = 1.0 + c3_mpower(chartInstance, c3_b);
c3_f_b = c3_qe[0];
c3_r_y = c3_f_a * c3_f_b;
c3_d_A = c3_q_y;
c3_e_B = c3_r_y;
c3_m_x = c3_d_A;
c3_s_y = c3_e_B;
c3_n_x = c3_m_x;
c3_t_y = c3_s_y;
c3_u_y = c3_n_x / c3_t_y;
c3_o_x = c3_qe[2];
c3_p_x = c3_o_x;
c3_p_x = muDoubleScalarSin(c3_p_x);
c3_g_a = c3_p_x;
c3_v_y = c3_g_a * 2.0;
c3_h_a = 1.0 + c3_mpower(chartInstance, c3_b);
c3_g_b = c3_qe[0];
c3_w_y = c3_h_a * c3_g_b;
c3_e_A = c3_v_y;
c3_f_B = c3_w_y;
c3_q_x = c3_e_A;
c3_x_y = c3_f_B;
c3_r_x = c3_q_x;
c3_y_y = c3_x_y;
c3_ab_y = c3_r_x / c3_y_y;
c3_b2 = c3_u_y - c3_ab_y;
_SFD_EML_CALL(0U, chartInstance->c3_sfEvent, 8);
c3_i_a = -c3_k1;
c3_h_b = c3_b1;
c3_bb_y = c3_i_a * c3_h_b;
c3_j_a = c3_bb_y;
c3_i_b = c3_z[0];
c3_v = c3_j_a * c3_i_b;
_SFD_EML_CALL(0U, chartInstance->c3_sfEvent, 9);
c3_k_a = -c3_b2;
c3_j_b = c3_v;
c3_cb_y = c3_k_a * c3_j_b;
c3_l_a = c3_k2;
c3_k_b = c3_z[1];
c3_db_y = c3_l_a * c3_k_b;
c3_w = c3_cb_y - c3_db_y;
_SFD_EML_CALL(0U, chartInstance->c3_sfEvent, -9);
sf_debug_symbol_scope_pop();
*c3_b_w = c3_w;
*c3_b_v = c3_v;
_SFD_CC_CALL(EXIT_OUT_OF_FUNCTION_TAG, 2U, chartInstance->c3_sfEvent);
sf_debug_check_for_state_inconsistency(_canudas_goal3MachineNumber_,
chartInstance->chartNumber, chartInstance->instanceNumber);
}
static void sf_c2_car_model(SFc2_car_modelInstanceStruct *chartInstance)
{
int32_T c2_i3;
int32_T c2_i4;
int32_T c2_i5;
int32_T c2_i6;
int32_T c2_i7;
int32_T c2_previousEvent;
int32_T c2_i8;
real_T c2_hoistedGlobal[4];
int32_T c2_i9;
real_T c2_b_hoistedGlobal[3];
int32_T c2_i10;
real_T c2_c_hoistedGlobal[2];
int32_T c2_i11;
real_T c2_d_hoistedGlobal[2];
int32_T c2_i12;
real_T c2_X[4];
int32_T c2_i13;
real_T c2_u[3];
int32_T c2_i14;
real_T c2_f_F[2];
int32_T c2_i15;
real_T c2_f_R[2];
uint32_T c2_debug_family_var_map[26];
real_T c2_m;
real_T c2_Iz;
real_T c2_l_F;
real_T c2_l_R;
real_T c2_Vx;
real_T c2_Vy;
real_T c2_r;
real_T c2_psi;
real_T c2_f_Fx;
real_T c2_f_Rx;
real_T c2_delta;
real_T c2_f_Fy;
real_T c2_f_Fz;
real_T c2_f_Ry;
real_T c2_f_Rz;
real_T c2_dVx;
real_T c2_dVy;
real_T c2_dr;
real_T c2_dpsi;
real_T c2_nargin = 4.0;
real_T c2_nargout = 1.0;
real_T c2_dX[4];
real_T c2_x;
real_T c2_b_x;
real_T c2_c_x;
real_T c2_a;
real_T c2_b;
real_T c2_y;
real_T c2_d_x;
real_T c2_e_x;
real_T c2_f_x;
real_T c2_b_a;
real_T c2_b_b;
real_T c2_b_y;
real_T c2_c_b;
real_T c2_c_y;
real_T c2_c_a;
real_T c2_d_b;
real_T c2_d_y;
real_T c2_g_x;
real_T c2_h_x;
real_T c2_i_x;
real_T c2_d_a;
real_T c2_e_b;
real_T c2_e_y;
real_T c2_j_x;
real_T c2_k_x;
real_T c2_l_x;
real_T c2_e_a;
real_T c2_f_b;
real_T c2_f_y;
real_T c2_g_b;
real_T c2_g_y;
real_T c2_f_a;
real_T c2_h_b;
real_T c2_h_y;
real_T c2_m_x;
real_T c2_n_x;
real_T c2_o_x;
real_T c2_g_a;
real_T c2_i_b;
real_T c2_i_y;
real_T c2_p_x;
real_T c2_q_x;
real_T c2_r_x;
real_T c2_h_a;
real_T c2_j_b;
real_T c2_j_y;
real_T c2_i_a;
real_T c2_k_y;
real_T c2_j_a;
real_T c2_l_y;
real_T c2_k_b;
real_T c2_b_dVx[4];
int32_T c2_i16;
int32_T c2_i17;
real_T (*c2_b_dX)[4];
real_T (*c2_b_f_R)[2];
real_T (*c2_b_f_F)[2];
real_T (*c2_b_u)[3];
real_T (*c2_b_X)[4];
c2_b_dX = (real_T (*)[4])ssGetOutputPortSignal(chartInstance->S, 1);
c2_b_f_R = (real_T (*)[2])ssGetInputPortSignal(chartInstance->S, 3);
c2_b_f_F = (real_T (*)[2])ssGetInputPortSignal(chartInstance->S, 2);
c2_b_u = (real_T (*)[3])ssGetInputPortSignal(chartInstance->S, 1);
c2_b_X = (real_T (*)[4])ssGetInputPortSignal(chartInstance->S, 0);
_sfTime_ = (real_T)ssGetT(chartInstance->S);
_SFD_CC_CALL(CHART_ENTER_SFUNCTION_TAG,1);
for (c2_i3 = 0; c2_i3 < 4; c2_i3 = c2_i3 + 1) {
_SFD_DATA_RANGE_CHECK((*c2_b_X)[c2_i3], 0U);
}
for (c2_i4 = 0; c2_i4 < 3; c2_i4 = c2_i4 + 1) {
_SFD_DATA_RANGE_CHECK((*c2_b_u)[c2_i4], 1U);
}
for (c2_i5 = 0; c2_i5 < 2; c2_i5 = c2_i5 + 1) {
_SFD_DATA_RANGE_CHECK((*c2_b_f_F)[c2_i5], 2U);
}
for (c2_i6 = 0; c2_i6 < 2; c2_i6 = c2_i6 + 1) {
_SFD_DATA_RANGE_CHECK((*c2_b_f_R)[c2_i6], 3U);
}
for (c2_i7 = 0; c2_i7 < 4; c2_i7 = c2_i7 + 1) {
_SFD_DATA_RANGE_CHECK((*c2_b_dX)[c2_i7], 4U);
}
c2_previousEvent = _sfEvent_;
_sfEvent_ = CALL_EVENT;
_SFD_CC_CALL(CHART_ENTER_DURING_FUNCTION_TAG,1);
for (c2_i8 = 0; c2_i8 < 4; c2_i8 = c2_i8 + 1) {
c2_hoistedGlobal[c2_i8] = (*c2_b_X)[c2_i8];
}
for (c2_i9 = 0; c2_i9 < 3; c2_i9 = c2_i9 + 1) {
c2_b_hoistedGlobal[c2_i9] = (*c2_b_u)[c2_i9];
}
for (c2_i10 = 0; c2_i10 < 2; c2_i10 = c2_i10 + 1) {
c2_c_hoistedGlobal[c2_i10] = (*c2_b_f_F)[c2_i10];
}
for (c2_i11 = 0; c2_i11 < 2; c2_i11 = c2_i11 + 1) {
c2_d_hoistedGlobal[c2_i11] = (*c2_b_f_R)[c2_i11];
}
for (c2_i12 = 0; c2_i12 < 4; c2_i12 = c2_i12 + 1) {
c2_X[c2_i12] = c2_hoistedGlobal[c2_i12];
}
for (c2_i13 = 0; c2_i13 < 3; c2_i13 = c2_i13 + 1) {
c2_u[c2_i13] = c2_b_hoistedGlobal[c2_i13];
}
for (c2_i14 = 0; c2_i14 < 2; c2_i14 = c2_i14 + 1) {
c2_f_F[c2_i14] = c2_c_hoistedGlobal[c2_i14];
}
for (c2_i15 = 0; c2_i15 < 2; c2_i15 = c2_i15 + 1) {
c2_f_R[c2_i15] = c2_d_hoistedGlobal[c2_i15];
}
sf_debug_symbol_scope_push_eml(0U, 26U, 26U, c2_debug_family_names,
c2_debug_family_var_map);
sf_debug_symbol_scope_add_eml(&c2_m, c2_d_sf_marshall, 0U);
sf_debug_symbol_scope_add_eml(&c2_Iz, c2_d_sf_marshall, 1U);
sf_debug_symbol_scope_add_eml(&c2_l_F, c2_d_sf_marshall, 2U);
sf_debug_symbol_scope_add_eml(&c2_l_R, c2_d_sf_marshall, 3U);
sf_debug_symbol_scope_add_eml(&c2_Vx, c2_d_sf_marshall, 4U);
sf_debug_symbol_scope_add_eml(&c2_Vy, c2_d_sf_marshall, 5U);
sf_debug_symbol_scope_add_eml(&c2_r, c2_d_sf_marshall, 6U);
sf_debug_symbol_scope_add_eml(&c2_psi, c2_d_sf_marshall, 7U);
sf_debug_symbol_scope_add_eml(&c2_f_Fx, c2_d_sf_marshall, 8U);
sf_debug_symbol_scope_add_eml(&c2_f_Rx, c2_d_sf_marshall, 9U);
sf_debug_symbol_scope_add_eml(&c2_delta, c2_d_sf_marshall, 10U);
sf_debug_symbol_scope_add_eml(&c2_f_Fy, c2_d_sf_marshall, 11U);
sf_debug_symbol_scope_add_eml(&c2_f_Fz, c2_d_sf_marshall, 12U);
sf_debug_symbol_scope_add_eml(&c2_f_Ry, c2_d_sf_marshall, 13U);
sf_debug_symbol_scope_add_eml(&c2_f_Rz, c2_d_sf_marshall, 14U);
sf_debug_symbol_scope_add_eml(&c2_dVx, c2_d_sf_marshall, 15U);
sf_debug_symbol_scope_add_eml(&c2_dVy, c2_d_sf_marshall, 16U);
sf_debug_symbol_scope_add_eml(&c2_dr, c2_d_sf_marshall, 17U);
sf_debug_symbol_scope_add_eml(&c2_dpsi, c2_d_sf_marshall, 18U);
sf_debug_symbol_scope_add_eml(&c2_nargin, c2_d_sf_marshall, 19U);
sf_debug_symbol_scope_add_eml(&c2_nargout, c2_d_sf_marshall, 20U);
sf_debug_symbol_scope_add_eml(&c2_X, c2_sf_marshall, 21U);
sf_debug_symbol_scope_add_eml(&c2_u, c2_c_sf_marshall, 22U);
sf_debug_symbol_scope_add_eml(&c2_f_F, c2_b_sf_marshall, 23U);
sf_debug_symbol_scope_add_eml(&c2_f_R, c2_b_sf_marshall, 24U);
sf_debug_symbol_scope_add_eml(&c2_dX, c2_sf_marshall, 25U);
CV_EML_FCN(0, 0);
/* m = param.m; */
/* Iz = param.Iz; */
/* l_F = param.l_F; */
/* l_R = param.l_R; */
/* B = param.B; */
/* C = param.C; */
/* mu = param.mu; */
/* delta_max = param.delta_max; */
_SFD_EML_CALL(0,13);
c2_m = 1450.0;
_SFD_EML_CALL(0,14);
c2_Iz = 2740.0;
_SFD_EML_CALL(0,15);
c2_l_F = 1.1;
_SFD_EML_CALL(0,16);
c2_l_R = 1.6;
_SFD_EML_CALL(0,18);
c2_Vx = c2_X[0];
_SFD_EML_CALL(0,19);
c2_Vy = c2_X[1];
_SFD_EML_CALL(0,20);
c2_r = c2_X[2];
_SFD_EML_CALL(0,21);
c2_psi = c2_X[3];
_SFD_EML_CALL(0,23);
c2_f_Fx = c2_u[0];
_SFD_EML_CALL(0,24);
c2_f_Rx = c2_u[1];
_SFD_EML_CALL(0,25);
c2_delta = c2_u[2];
_SFD_EML_CALL(0,27);
c2_f_Fy = c2_f_F[0];
_SFD_EML_CALL(0,28);
c2_f_Fz = c2_f_F[1];
_SFD_EML_CALL(0,30);
c2_f_Ry = c2_f_R[0];
_SFD_EML_CALL(0,31);
c2_f_Rz = c2_f_R[1];
_SFD_EML_CALL(0,34);
c2_x = c2_delta;
c2_b_x = c2_x;
c2_c_x = c2_b_x;
c2_b_x = c2_c_x;
c2_b_x = muDoubleScalarCos(c2_b_x);
c2_a = c2_f_Fx;
c2_b = c2_b_x;
c2_y = c2_a * c2_b;
c2_d_x = c2_delta;
c2_e_x = c2_d_x;
c2_f_x = c2_e_x;
c2_e_x = c2_f_x;
c2_e_x = muDoubleScalarSin(c2_e_x);
c2_b_a = c2_f_Fy;
c2_b_b = c2_e_x;
c2_b_y = c2_b_a * c2_b_b;
c2_c_b = (c2_y - c2_b_y) + c2_f_Rx;
c2_c_y = 6.8965517241379305E-004 * c2_c_b;
c2_c_a = c2_Vy;
c2_d_b = c2_r;
c2_d_y = c2_c_a * c2_d_b;
c2_dVx = c2_c_y + c2_d_y;
_SFD_EML_CALL(0,35);
c2_g_x = c2_delta;
c2_h_x = c2_g_x;
c2_i_x = c2_h_x;
c2_h_x = c2_i_x;
c2_h_x = muDoubleScalarSin(c2_h_x);
c2_d_a = c2_f_Fx;
c2_e_b = c2_h_x;
c2_e_y = c2_d_a * c2_e_b;
c2_j_x = c2_delta;
c2_k_x = c2_j_x;
c2_l_x = c2_k_x;
c2_k_x = c2_l_x;
c2_k_x = muDoubleScalarCos(c2_k_x);
c2_e_a = c2_f_Fy;
c2_f_b = c2_k_x;
c2_f_y = c2_e_a * c2_f_b;
c2_g_b = (c2_e_y + c2_f_y) + c2_f_Ry;
c2_g_y = 6.8965517241379305E-004 * c2_g_b;
c2_f_a = c2_Vx;
c2_h_b = c2_r;
c2_h_y = c2_f_a * c2_h_b;
c2_dVy = c2_g_y - c2_h_y;
_SFD_EML_CALL(0,36);
c2_m_x = c2_delta;
c2_n_x = c2_m_x;
c2_o_x = c2_n_x;
c2_n_x = c2_o_x;
c2_n_x = muDoubleScalarCos(c2_n_x);
c2_g_a = c2_f_Fy;
c2_i_b = c2_n_x;
c2_i_y = c2_g_a * c2_i_b;
c2_p_x = c2_delta;
c2_q_x = c2_p_x;
c2_r_x = c2_q_x;
c2_q_x = c2_r_x;
c2_q_x = muDoubleScalarSin(c2_q_x);
c2_h_a = c2_f_Fx;
c2_j_b = c2_q_x;
c2_j_y = c2_h_a * c2_j_b;
c2_i_a = c2_i_y + c2_j_y;
c2_k_y = c2_i_a * 1.1;
c2_j_a = c2_f_Ry;
c2_l_y = c2_j_a * 1.6;
c2_k_b = c2_k_y - c2_l_y;
c2_dr = 3.6496350364963501E-004 * c2_k_b;
_SFD_EML_CALL(0,37);
c2_dpsi = c2_r;
_SFD_EML_CALL(0,39);
c2_b_dVx[0] = c2_dVx;
c2_b_dVx[1] = c2_dVy;
c2_b_dVx[2] = c2_dr;
c2_b_dVx[3] = c2_dpsi;
for (c2_i16 = 0; c2_i16 < 4; c2_i16 = c2_i16 + 1) {
c2_dX[c2_i16] = c2_b_dVx[c2_i16];
}
_SFD_EML_CALL(0,-39);
sf_debug_symbol_scope_pop();
for (c2_i17 = 0; c2_i17 < 4; c2_i17 = c2_i17 + 1) {
(*c2_b_dX)[c2_i17] = c2_dX[c2_i17];
}
_SFD_CC_CALL(EXIT_OUT_OF_FUNCTION_TAG,1);
_sfEvent_ = c2_previousEvent;
sf_debug_check_for_state_inconsistency(_car_modelMachineNumber_,
chartInstance->chartNumber, chartInstance->instanceNumber);
}
static void c6_chartstep_c6_motor_control(SFc6_motor_controlInstanceStruct
*chartInstance)
{
real_T c6_hoistedGlobal;
real_T c6_b_hoistedGlobal;
real_T c6_c_hoistedGlobal;
real_T c6_Xd;
real_T c6_Xq;
real_T c6_theta;
uint32_T c6_debug_family_var_map[11];
real_T c6_c;
real_T c6_s;
real_T c6_ipm[4];
real_T c6_xab[2];
real_T c6_nargin = 3.0;
real_T c6_nargout = 2.0;
real_T c6_Xalpha;
real_T c6_Xbeta;
real_T c6_x;
real_T c6_b_x;
real_T c6_c_x;
real_T c6_d_x;
int32_T c6_i0;
real_T c6_a[4];
real_T c6_b_Xd[2];
int32_T c6_i1;
real_T c6_b[2];
int32_T c6_i2;
int32_T c6_i3;
int32_T c6_i4;
real_T c6_C[2];
int32_T c6_i5;
int32_T c6_i6;
int32_T c6_i7;
int32_T c6_i8;
int32_T c6_i9;
int32_T c6_i10;
real_T *c6_c_Xd;
real_T *c6_b_Xq;
real_T *c6_b_theta;
real_T *c6_b_Xalpha;
real_T *c6_b_Xbeta;
c6_b_Xbeta = (real_T *)ssGetOutputPortSignal(chartInstance->S, 2);
c6_b_theta = (real_T *)ssGetInputPortSignal(chartInstance->S, 2);
c6_b_Xq = (real_T *)ssGetInputPortSignal(chartInstance->S, 1);
c6_b_Xalpha = (real_T *)ssGetOutputPortSignal(chartInstance->S, 1);
c6_c_Xd = (real_T *)ssGetInputPortSignal(chartInstance->S, 0);
_SFD_CC_CALL(CHART_ENTER_DURING_FUNCTION_TAG, 5U, chartInstance->c6_sfEvent);
c6_hoistedGlobal = *c6_c_Xd;
c6_b_hoistedGlobal = *c6_b_Xq;
c6_c_hoistedGlobal = *c6_b_theta;
c6_Xd = c6_hoistedGlobal;
c6_Xq = c6_b_hoistedGlobal;
c6_theta = c6_c_hoistedGlobal;
sf_debug_symbol_scope_push_eml(0U, 11U, 11U, c6_debug_family_names,
c6_debug_family_var_map);
sf_debug_symbol_scope_add_eml_importable(&c6_c, 0U, c6_sf_marshallOut,
c6_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c6_s, 1U, c6_sf_marshallOut,
c6_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(c6_ipm, 2U, c6_c_sf_marshallOut,
c6_c_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(c6_xab, 3U, c6_b_sf_marshallOut,
c6_b_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c6_nargin, 4U, c6_sf_marshallOut,
c6_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c6_nargout, 5U, c6_sf_marshallOut,
c6_sf_marshallIn);
sf_debug_symbol_scope_add_eml(&c6_Xd, 6U, c6_sf_marshallOut);
sf_debug_symbol_scope_add_eml(&c6_Xq, 7U, c6_sf_marshallOut);
sf_debug_symbol_scope_add_eml(&c6_theta, 8U, c6_sf_marshallOut);
sf_debug_symbol_scope_add_eml_importable(&c6_Xalpha, 9U, c6_sf_marshallOut,
c6_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c6_Xbeta, 10U, c6_sf_marshallOut,
c6_sf_marshallIn);
CV_EML_FCN(0, 0);
_SFD_EML_CALL(0U, chartInstance->c6_sfEvent, 5);
c6_x = c6_theta;
c6_c = c6_x;
c6_b_x = c6_c;
c6_c = c6_b_x;
c6_c = muDoubleScalarCos(c6_c);
_SFD_EML_CALL(0U, chartInstance->c6_sfEvent, 6);
c6_c_x = c6_theta;
c6_s = c6_c_x;
c6_d_x = c6_s;
c6_s = c6_d_x;
c6_s = muDoubleScalarSin(c6_s);
_SFD_EML_CALL(0U, chartInstance->c6_sfEvent, 8);
c6_ipm[0] = c6_c;
c6_ipm[2] = -c6_s;
c6_ipm[1] = c6_s;
c6_ipm[3] = c6_c;
_SFD_EML_CALL(0U, chartInstance->c6_sfEvent, 10);
for (c6_i0 = 0; c6_i0 < 4; c6_i0++) {
c6_a[c6_i0] = c6_ipm[c6_i0];
}
c6_b_Xd[0] = c6_Xd;
c6_b_Xd[1] = c6_Xq;
for (c6_i1 = 0; c6_i1 < 2; c6_i1++) {
c6_b[c6_i1] = c6_b_Xd[c6_i1];
}
c6_eml_scalar_eg(chartInstance);
c6_eml_scalar_eg(chartInstance);
for (c6_i2 = 0; c6_i2 < 2; c6_i2++) {
c6_xab[c6_i2] = 0.0;
}
for (c6_i3 = 0; c6_i3 < 2; c6_i3++) {
c6_xab[c6_i3] = 0.0;
}
for (c6_i4 = 0; c6_i4 < 2; c6_i4++) {
c6_C[c6_i4] = c6_xab[c6_i4];
}
for (c6_i5 = 0; c6_i5 < 2; c6_i5++) {
c6_xab[c6_i5] = c6_C[c6_i5];
}
for (c6_i6 = 0; c6_i6 < 2; c6_i6++) {
c6_C[c6_i6] = c6_xab[c6_i6];
}
for (c6_i7 = 0; c6_i7 < 2; c6_i7++) {
c6_xab[c6_i7] = c6_C[c6_i7];
}
for (c6_i8 = 0; c6_i8 < 2; c6_i8++) {
c6_xab[c6_i8] = 0.0;
c6_i9 = 0;
for (c6_i10 = 0; c6_i10 < 2; c6_i10++) {
c6_xab[c6_i8] += c6_a[c6_i9 + c6_i8] * c6_b[c6_i10];
c6_i9 += 2;
}
}
_SFD_EML_CALL(0U, chartInstance->c6_sfEvent, 12);
c6_Xalpha = c6_xab[0];
_SFD_EML_CALL(0U, chartInstance->c6_sfEvent, 13);
c6_Xbeta = c6_xab[1];
_SFD_EML_CALL(0U, chartInstance->c6_sfEvent, -13);
sf_debug_symbol_scope_pop();
*c6_b_Xalpha = c6_Xalpha;
*c6_b_Xbeta = c6_Xbeta;
_SFD_CC_CALL(EXIT_OUT_OF_FUNCTION_TAG, 5U, chartInstance->c6_sfEvent);
}
static void sf_gateway_c8_AllPurposeModel_TyreRelaxation
(SFc8_AllPurposeModel_TyreRelaxationInstanceStruct *chartInstance)
{
real_T c8_hoistedGlobal;
c8_irp_vec3 c8_b_v_irp3;
real_T c8_b_p_z;
uint32_T c8_debug_family_var_map[5];
real_T c8_nargin = 2.0;
real_T c8_nargout = 1.0;
c8_irp_vec3 c8_b_derpos_irp3;
real_T c8_x;
real_T c8_b_x;
real_T c8_c_x;
real_T c8_d_x;
real_T c8_e_x;
real_T c8_f_x;
real_T c8_g_x;
real_T c8_h_x;
_SFD_SYMBOL_SCOPE_PUSH(0U, 0U);
_sfTime_ = sf_get_time(chartInstance->S);
_SFD_CC_CALL(CHART_ENTER_SFUNCTION_TAG, 7U, chartInstance->c8_sfEvent);
_SFD_DATA_RANGE_CHECK(*chartInstance->c8_p_z, 1U);
chartInstance->c8_sfEvent = CALL_EVENT;
_SFD_CC_CALL(CHART_ENTER_DURING_FUNCTION_TAG, 7U, chartInstance->c8_sfEvent);
c8_hoistedGlobal = *chartInstance->c8_p_z;
c8_b_v_irp3.x = *(real_T *)&((char_T *)chartInstance->c8_v_irp3)[0];
c8_b_v_irp3.y = *(real_T *)&((char_T *)chartInstance->c8_v_irp3)[8];
c8_b_v_irp3.rotz = *(real_T *)&((char_T *)chartInstance->c8_v_irp3)[16];
c8_b_p_z = c8_hoistedGlobal;
_SFD_SYMBOL_SCOPE_PUSH_EML(0U, 5U, 5U, c8_debug_family_names,
c8_debug_family_var_map);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c8_nargin, 0U, c8_b_sf_marshallOut,
c8_b_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c8_nargout, 1U, c8_b_sf_marshallOut,
c8_b_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML(&c8_b_v_irp3, 2U, c8_sf_marshallOut);
_SFD_SYMBOL_SCOPE_ADD_EML(&c8_b_p_z, 3U, c8_b_sf_marshallOut);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c8_b_derpos_irp3, 4U, c8_sf_marshallOut,
c8_sf_marshallIn);
CV_EML_FCN(0, 0);
_SFD_EML_CALL(0U, chartInstance->c8_sfEvent, 4);
c8_x = c8_b_p_z;
c8_b_x = c8_x;
c8_b_x = muDoubleScalarCos(c8_b_x);
c8_c_x = c8_b_p_z;
c8_d_x = c8_c_x;
c8_d_x = muDoubleScalarSin(c8_d_x);
c8_b_derpos_irp3.x = c8_b_x * c8_b_v_irp3.x - c8_d_x * c8_b_v_irp3.y;
_SFD_EML_CALL(0U, chartInstance->c8_sfEvent, 5);
c8_e_x = c8_b_p_z;
c8_f_x = c8_e_x;
c8_f_x = muDoubleScalarSin(c8_f_x);
c8_g_x = c8_b_p_z;
c8_h_x = c8_g_x;
c8_h_x = muDoubleScalarCos(c8_h_x);
c8_b_derpos_irp3.y = c8_f_x * c8_b_v_irp3.x + c8_h_x * c8_b_v_irp3.y;
_SFD_EML_CALL(0U, chartInstance->c8_sfEvent, 6);
c8_b_derpos_irp3.rotz = c8_b_v_irp3.rotz;
_SFD_EML_CALL(0U, chartInstance->c8_sfEvent, -6);
_SFD_SYMBOL_SCOPE_POP();
*(real_T *)&((char_T *)chartInstance->c8_derpos_irp3)[0] = c8_b_derpos_irp3.x;
*(real_T *)&((char_T *)chartInstance->c8_derpos_irp3)[8] = c8_b_derpos_irp3.y;
*(real_T *)&((char_T *)chartInstance->c8_derpos_irp3)[16] =
c8_b_derpos_irp3.rotz;
_SFD_CC_CALL(EXIT_OUT_OF_FUNCTION_TAG, 7U, chartInstance->c8_sfEvent);
_SFD_SYMBOL_SCOPE_POP();
_SFD_CHECK_FOR_STATE_INCONSISTENCY
(_AllPurposeModel_TyreRelaxationMachineNumber_, chartInstance->chartNumber,
chartInstance->instanceNumber);
}
static void sf_c3_car_model(SFc3_car_modelInstanceStruct *chartInstance)
{
int32_T c3_i3;
int32_T c3_i4;
int32_T c3_i5;
int32_T c3_previousEvent;
int32_T c3_i6;
real_T c3_hoistedGlobal[4];
int32_T c3_i7;
real_T c3_b_hoistedGlobal[3];
int32_T c3_i8;
real_T c3_X[4];
int32_T c3_i9;
real_T c3_u[3];
uint32_T c3_debug_family_var_map[22];
real_T c3_l_F;
real_T c3_l_R;
real_T c3_Vx;
real_T c3_Vy;
real_T c3_r;
real_T c3_psi;
real_T c3_f_Fx;
real_T c3_f_Rx;
real_T c3_delta;
real_T c3_beta;
real_T c3_V;
real_T c3_s_Fy_num;
real_T c3_s_Fy_denum;
real_T c3_s_Fy;
real_T c3_s_Ry_num;
real_T c3_s_Ry_denum;
real_T c3_s_Ry;
real_T c3_nargin = 2.0;
real_T c3_nargout = 1.0;
real_T c3_s[2];
real_T c3_A;
real_T c3_B;
real_T c3_x;
real_T c3_y;
real_T c3_b_x;
real_T c3_b_y;
real_T c3_c_x;
real_T c3_c_y;
real_T c3_d_y;
real_T c3_d_x;
real_T c3_e_x;
real_T c3_f_x;
real_T c3_g_x;
real_T c3_h_x;
real_T c3_i_x;
real_T c3_j_x;
real_T c3_a;
real_T c3_b;
real_T c3_e_y;
real_T c3_b_a;
real_T c3_f_y;
real_T c3_k_x;
real_T c3_l_x;
real_T c3_m_x;
real_T c3_c_a;
real_T c3_b_b;
real_T c3_g_y;
real_T c3_n_x;
real_T c3_o_x;
real_T c3_p_x;
real_T c3_d_a;
real_T c3_c_b;
real_T c3_h_y;
real_T c3_e_a;
real_T c3_i_y;
real_T c3_q_x;
real_T c3_r_x;
real_T c3_s_x;
real_T c3_f_a;
real_T c3_d_b;
real_T c3_j_y;
real_T c3_b_A;
real_T c3_b_B;
real_T c3_t_x;
real_T c3_k_y;
real_T c3_u_x;
real_T c3_l_y;
real_T c3_v_x;
real_T c3_m_y;
real_T c3_w_x;
real_T c3_x_x;
real_T c3_y_x;
real_T c3_g_a;
real_T c3_e_b;
real_T c3_n_y;
real_T c3_h_a;
real_T c3_o_y;
real_T c3_ab_x;
real_T c3_bb_x;
real_T c3_cb_x;
real_T c3_i_a;
real_T c3_f_b;
real_T c3_c_A;
real_T c3_c_B;
real_T c3_db_x;
real_T c3_p_y;
real_T c3_eb_x;
real_T c3_q_y;
real_T c3_fb_x;
real_T c3_r_y;
int32_T c3_i10;
real_T (*c3_b_s)[2];
real_T (*c3_b_u)[3];
real_T (*c3_b_X)[4];
c3_b_u = (real_T (*)[3])ssGetInputPortSignal(chartInstance->S, 1);
c3_b_s = (real_T (*)[2])ssGetOutputPortSignal(chartInstance->S, 1);
c3_b_X = (real_T (*)[4])ssGetInputPortSignal(chartInstance->S, 0);
_sfTime_ = (real_T)ssGetT(chartInstance->S);
_SFD_CC_CALL(CHART_ENTER_SFUNCTION_TAG,2);
for (c3_i3 = 0; c3_i3 < 4; c3_i3 = c3_i3 + 1) {
_SFD_DATA_RANGE_CHECK((*c3_b_X)[c3_i3], 0U);
}
for (c3_i4 = 0; c3_i4 < 2; c3_i4 = c3_i4 + 1) {
_SFD_DATA_RANGE_CHECK((*c3_b_s)[c3_i4], 1U);
}
for (c3_i5 = 0; c3_i5 < 3; c3_i5 = c3_i5 + 1) {
_SFD_DATA_RANGE_CHECK((*c3_b_u)[c3_i5], 2U);
}
c3_previousEvent = _sfEvent_;
_sfEvent_ = CALL_EVENT;
_SFD_CC_CALL(CHART_ENTER_DURING_FUNCTION_TAG,2);
for (c3_i6 = 0; c3_i6 < 4; c3_i6 = c3_i6 + 1) {
c3_hoistedGlobal[c3_i6] = (*c3_b_X)[c3_i6];
}
for (c3_i7 = 0; c3_i7 < 3; c3_i7 = c3_i7 + 1) {
c3_b_hoistedGlobal[c3_i7] = (*c3_b_u)[c3_i7];
}
for (c3_i8 = 0; c3_i8 < 4; c3_i8 = c3_i8 + 1) {
c3_X[c3_i8] = c3_hoistedGlobal[c3_i8];
}
for (c3_i9 = 0; c3_i9 < 3; c3_i9 = c3_i9 + 1) {
c3_u[c3_i9] = c3_b_hoistedGlobal[c3_i9];
}
sf_debug_symbol_scope_push_eml(0U, 22U, 22U, c3_debug_family_names,
c3_debug_family_var_map);
sf_debug_symbol_scope_add_eml(&c3_l_F, c3_d_sf_marshall, 0U);
sf_debug_symbol_scope_add_eml(&c3_l_R, c3_d_sf_marshall, 1U);
sf_debug_symbol_scope_add_eml(&c3_Vx, c3_d_sf_marshall, 2U);
sf_debug_symbol_scope_add_eml(&c3_Vy, c3_d_sf_marshall, 3U);
sf_debug_symbol_scope_add_eml(&c3_r, c3_d_sf_marshall, 4U);
sf_debug_symbol_scope_add_eml(&c3_psi, c3_d_sf_marshall, 5U);
sf_debug_symbol_scope_add_eml(&c3_f_Fx, c3_d_sf_marshall, 6U);
sf_debug_symbol_scope_add_eml(&c3_f_Rx, c3_d_sf_marshall, 7U);
sf_debug_symbol_scope_add_eml(&c3_delta, c3_d_sf_marshall, 8U);
sf_debug_symbol_scope_add_eml(&c3_beta, c3_d_sf_marshall, 9U);
sf_debug_symbol_scope_add_eml(&c3_V, c3_d_sf_marshall, 10U);
sf_debug_symbol_scope_add_eml(&c3_s_Fy_num, c3_d_sf_marshall, 11U);
sf_debug_symbol_scope_add_eml(&c3_s_Fy_denum, c3_d_sf_marshall, 12U);
sf_debug_symbol_scope_add_eml(&c3_s_Fy, c3_d_sf_marshall, 13U);
sf_debug_symbol_scope_add_eml(&c3_s_Ry_num, c3_d_sf_marshall, 14U);
sf_debug_symbol_scope_add_eml(&c3_s_Ry_denum, c3_d_sf_marshall, 15U);
sf_debug_symbol_scope_add_eml(&c3_s_Ry, c3_d_sf_marshall, 16U);
sf_debug_symbol_scope_add_eml(&c3_nargin, c3_d_sf_marshall, 17U);
sf_debug_symbol_scope_add_eml(&c3_nargout, c3_d_sf_marshall, 18U);
sf_debug_symbol_scope_add_eml(&c3_X, c3_c_sf_marshall, 19U);
sf_debug_symbol_scope_add_eml(&c3_u, c3_b_sf_marshall, 20U);
sf_debug_symbol_scope_add_eml(&c3_s, c3_sf_marshall, 21U);
CV_EML_FCN(0, 0);
/* l_F = param.l_F; */
/* l_R = param.l_R; */
_SFD_EML_CALL(0,7);
c3_l_F = 1.1;
_SFD_EML_CALL(0,8);
c3_l_R = 1.6;
_SFD_EML_CALL(0,10);
c3_Vx = c3_X[0];
_SFD_EML_CALL(0,11);
c3_Vy = c3_X[1];
_SFD_EML_CALL(0,12);
c3_r = c3_X[2];
_SFD_EML_CALL(0,13);
c3_psi = c3_X[3];
_SFD_EML_CALL(0,15);
c3_f_Fx = c3_u[0];
_SFD_EML_CALL(0,16);
c3_f_Rx = c3_u[1];
_SFD_EML_CALL(0,17);
c3_delta = c3_u[2];
_SFD_EML_CALL(0,20);
if (CV_EML_COND(0, 0, c3_Vx == 0.0)) {
if (CV_EML_COND(0, 1, c3_Vy == 0.0)) {
CV_EML_MCDC(0, 0, TRUE);
CV_EML_IF(0, 0, TRUE);
_SFD_EML_CALL(0,21);
c3_beta = 0.0;
goto label_1;
}
}
CV_EML_MCDC(0, 0, FALSE);
CV_EML_IF(0, 0, FALSE);
_SFD_EML_CALL(0,23);
c3_A = c3_Vy;
c3_B = c3_Vx;
c3_x = c3_A;
c3_y = c3_B;
c3_b_x = c3_x;
c3_b_y = c3_y;
c3_c_x = c3_b_x;
c3_c_y = c3_b_y;
c3_d_y = c3_c_x / c3_c_y;
c3_d_x = c3_d_y;
c3_beta = c3_d_x;
c3_e_x = c3_beta;
c3_beta = c3_e_x;
c3_beta = muDoubleScalarAtan(c3_beta);
label_1:
;
_SFD_EML_CALL(0,26);
c3_f_x = c3_mpower(chartInstance, c3_Vx) + c3_mpower(chartInstance, c3_Vy);
c3_V = c3_f_x;
if (c3_V < 0.0) {
c3_eml_error(chartInstance);
}
c3_g_x = c3_V;
c3_V = c3_g_x;
c3_V = muDoubleScalarSqrt(c3_V);
_SFD_EML_CALL(0,28);
c3_h_x = c3_beta - c3_delta;
c3_i_x = c3_h_x;
c3_j_x = c3_i_x;
c3_i_x = c3_j_x;
c3_i_x = muDoubleScalarSin(c3_i_x);
c3_a = c3_V;
c3_b = c3_i_x;
c3_e_y = c3_a * c3_b;
c3_b_a = c3_r;
c3_f_y = c3_b_a * 1.1;
c3_k_x = c3_delta;
c3_l_x = c3_k_x;
c3_m_x = c3_l_x;
c3_l_x = c3_m_x;
c3_l_x = muDoubleScalarCos(c3_l_x);
c3_c_a = c3_f_y;
c3_b_b = c3_l_x;
c3_g_y = c3_c_a * c3_b_b;
c3_s_Fy_num = c3_e_y + c3_g_y;
_SFD_EML_CALL(0,29);
c3_n_x = c3_beta - c3_delta;
c3_o_x = c3_n_x;
c3_p_x = c3_o_x;
c3_o_x = c3_p_x;
c3_o_x = muDoubleScalarCos(c3_o_x);
c3_d_a = c3_V;
c3_c_b = c3_o_x;
c3_h_y = c3_d_a * c3_c_b;
c3_e_a = c3_r;
c3_i_y = c3_e_a * 1.1;
c3_q_x = c3_delta;
c3_r_x = c3_q_x;
c3_s_x = c3_r_x;
c3_r_x = c3_s_x;
c3_r_x = muDoubleScalarSin(c3_r_x);
c3_f_a = c3_i_y;
c3_d_b = c3_r_x;
c3_j_y = c3_f_a * c3_d_b;
c3_s_Fy_denum = c3_h_y + c3_j_y;
_SFD_EML_CALL(0,31);
if (CV_EML_COND(0, 2, c3_s_Fy_num == 0.0)) {
if (CV_EML_COND(0, 3, c3_s_Fy_denum == 0.0)) {
CV_EML_MCDC(0, 1, TRUE);
CV_EML_IF(0, 1, TRUE);
_SFD_EML_CALL(0,32);
c3_s_Fy = 0.0;
goto label_2;
}
}
CV_EML_MCDC(0, 1, FALSE);
CV_EML_IF(0, 1, FALSE);
_SFD_EML_CALL(0,34);
c3_b_A = c3_s_Fy_num;
c3_b_B = c3_s_Fy_denum;
c3_t_x = c3_b_A;
c3_k_y = c3_b_B;
c3_u_x = c3_t_x;
c3_l_y = c3_k_y;
c3_v_x = c3_u_x;
c3_m_y = c3_l_y;
c3_s_Fy = c3_v_x / c3_m_y;
label_2:
;
_SFD_EML_CALL(0,37);
c3_w_x = c3_beta;
c3_x_x = c3_w_x;
c3_y_x = c3_x_x;
c3_x_x = c3_y_x;
c3_x_x = muDoubleScalarSin(c3_x_x);
c3_g_a = c3_V;
c3_e_b = c3_x_x;
c3_n_y = c3_g_a * c3_e_b;
c3_h_a = c3_r;
c3_o_y = c3_h_a * 1.6;
c3_s_Ry_num = c3_n_y - c3_o_y;
_SFD_EML_CALL(0,38);
c3_ab_x = c3_beta;
c3_bb_x = c3_ab_x;
c3_cb_x = c3_bb_x;
c3_bb_x = c3_cb_x;
c3_bb_x = muDoubleScalarCos(c3_bb_x);
c3_i_a = c3_V;
c3_f_b = c3_bb_x;
c3_s_Ry_denum = c3_i_a * c3_f_b;
_SFD_EML_CALL(0,40);
if (CV_EML_COND(0, 4, c3_s_Ry_num == 0.0)) {
if (CV_EML_COND(0, 5, c3_s_Ry_denum == 0.0)) {
CV_EML_MCDC(0, 2, TRUE);
CV_EML_IF(0, 2, TRUE);
_SFD_EML_CALL(0,41);
c3_s_Ry = 0.0;
goto label_3;
}
}
CV_EML_MCDC(0, 2, FALSE);
CV_EML_IF(0, 2, FALSE);
_SFD_EML_CALL(0,43);
c3_c_A = c3_s_Ry_num;
c3_c_B = c3_s_Ry_denum;
c3_db_x = c3_c_A;
c3_p_y = c3_c_B;
c3_eb_x = c3_db_x;
c3_q_y = c3_p_y;
c3_fb_x = c3_eb_x;
c3_r_y = c3_q_y;
c3_s_Ry = c3_fb_x / c3_r_y;
label_3:
;
_SFD_EML_CALL(0,46);
c3_s[0] = c3_s_Fy;
c3_s[1] = c3_s_Ry;
_SFD_EML_CALL(0,-46);
sf_debug_symbol_scope_pop();
for (c3_i10 = 0; c3_i10 < 2; c3_i10 = c3_i10 + 1) {
(*c3_b_s)[c3_i10] = c3_s[c3_i10];
}
_SFD_CC_CALL(EXIT_OUT_OF_FUNCTION_TAG,2);
_sfEvent_ = c3_previousEvent;
sf_debug_check_for_state_inconsistency(_car_modelMachineNumber_,
chartInstance->chartNumber, chartInstance->instanceNumber);
}
void d_SystemCore_step(const emlrtStack *sp, c_commcodegen_PhaseFrequencyOff
*obj, const creal_T varargin_1[1408], real_T varargin_2, creal_T varargout_1
[1408])
{
static const char_T cv22[4] = { 's', 't', 'e', 'p' };
c_commcodegen_PhaseFrequencyOff *b_obj;
static const char_T cv23[5] = { 's', 'e', 't', 'u', 'p' };
comm_PhaseFrequencyOffset_2 *c_obj;
int32_T spfIdx;
real_T omegaT_plus_phi;
emlrtStack st;
emlrtStack b_st;
emlrtStack c_st;
emlrtStack d_st;
st.prev = sp;
st.tls = sp->tls;
b_st.prev = &st;
b_st.tls = st.tls;
c_st.prev = &b_st;
c_st.tls = b_st.tls;
d_st.prev = &c_st;
d_st.tls = c_st.tls;
if (obj->isInitialized != 2) {
} else {
emlrtErrorWithMessageIdR2012b(sp, &e_emlrtRTEI,
"MATLAB:system:methodCalledWhenReleasedCodegen", 3, 4, 4, cv22);
}
if (obj->isInitialized != 1) {
st.site = &u_emlrtRSI;
b_obj = obj;
b_st.site = &u_emlrtRSI;
if (b_obj->isInitialized == 0) {
} else {
emlrtErrorWithMessageIdR2012b(&b_st, &e_emlrtRTEI,
"MATLAB:system:methodCalledWhenLockedReleasedCodegen", 3, 4, 5, cv23);
}
b_obj->isInitialized = 1;
c_st.site = &u_emlrtRSI;
d_st.site = &eb_emlrtRSI;
if (b_obj->cSFunObject.S0_isInitialized != 1) {
if (b_obj->cSFunObject.S0_isInitialized == 2) {
emlrtErrorWithMessageIdR2012b(&d_st, &j_emlrtRTEI,
"MATLAB:system:setupCalledWhenLockedReleasedCodegen", 0);
}
b_obj->cSFunObject.S0_isInitialized = 1;
} else {
emlrtErrorWithMessageIdR2012b(&d_st, &j_emlrtRTEI,
"MATLAB:system:setupCalledWhenLockedCodegen", 0);
}
b_obj->c_NoTuningBeforeLockingCodeGenE = true;
b_st.site = &u_emlrtRSI;
c_st.site = &eb_emlrtRSI;
if (b_obj->cSFunObject.S0_isInitialized == 2) {
emlrtErrorWithMessageIdR2012b(&c_st, &i_emlrtRTEI,
"MATLAB:system:runtimeMethodCalledWhenReleasedCodegen", 0);
}
d_st.site = NULL;
/* System object Initialization function: comm.PhaseFrequencyOffset */
b_obj->cSFunObject.W0_omegatTnow = 0.0;
}
st.site = &u_emlrtRSI;
b_obj = obj;
b_st.site = &bb_emlrtRSI;
c_st.site = &eb_emlrtRSI;
c_obj = &b_obj->cSFunObject;
d_st.site = NULL;
/* System object Outputs function: comm.PhaseFrequencyOffset */
for (spfIdx = 0; spfIdx < 1408; spfIdx++) {
/* Compute (w*t + phi) argument for calls to cos and sin below */
omegaT_plus_phi = 6.2831853071795862 * c_obj->W0_omegatTnow +
c_obj->P0_Phase;
varargout_1[spfIdx].re = varargin_1[spfIdx].re * muDoubleScalarCos
(omegaT_plus_phi) - varargin_1[spfIdx].im * muDoubleScalarSin
(omegaT_plus_phi);
varargout_1[spfIdx].im = varargin_1[spfIdx].re * muDoubleScalarSin
(omegaT_plus_phi) + varargin_1[spfIdx].im * muDoubleScalarCos
(omegaT_plus_phi);
c_obj->W0_omegatTnow += varargin_2 * 5.0E-6;
}
b_st.site = &bb_emlrtRSI;
c_st.site = &eb_emlrtRSI;
if (b_obj->cSFunObject.S0_isInitialized != 1) {
emlrtErrorWithMessageIdR2012b(&c_st, &h_emlrtRTEI,
"MATLAB:system:updateCalledBeforeSetup", 0);
}
}
static void sf_c7_ekffedepre(SFc7_ekffedepreInstanceStruct *chartInstance)
{
real_T c7_hoistedGlobal;
real_T c7_b_hoistedGlobal;
real_T c7_c_hoistedGlobal;
real_T c7_d_hoistedGlobal;
real_T c7_e_hoistedGlobal;
real_T c7_sfasamento;
real_T c7_ampiezza;
real_T c7_frequenza;
real_T c7_noise;
real_T c7_t;
uint32_T c7_debug_family_var_map[8];
real_T c7_nargin = 5.0;
real_T c7_nargout = 1.0;
real_T c7_wave;
real_T c7_b;
real_T c7_y;
real_T c7_a;
real_T c7_b_b;
real_T c7_b_y;
real_T c7_x;
real_T c7_b_x;
real_T c7_b_a;
real_T c7_c_b;
real_T c7_c_y;
real_T *c7_b_sfasamento;
real_T *c7_b_wave;
real_T *c7_b_ampiezza;
real_T *c7_b_frequenza;
real_T *c7_b_noise;
real_T *c7_b_t;
c7_b_t = (real_T *)ssGetInputPortSignal(chartInstance->S, 4);
c7_b_noise = (real_T *)ssGetInputPortSignal(chartInstance->S, 3);
c7_b_frequenza = (real_T *)ssGetInputPortSignal(chartInstance->S, 2);
c7_b_ampiezza = (real_T *)ssGetInputPortSignal(chartInstance->S, 1);
c7_b_wave = (real_T *)ssGetOutputPortSignal(chartInstance->S, 1);
c7_b_sfasamento = (real_T *)ssGetInputPortSignal(chartInstance->S, 0);
_sfTime_ = (real_T)ssGetT(chartInstance->S);
_SFD_CC_CALL(CHART_ENTER_SFUNCTION_TAG, 6U, chartInstance->c7_sfEvent);
_SFD_DATA_RANGE_CHECK(*c7_b_sfasamento, 0U);
_SFD_DATA_RANGE_CHECK(*c7_b_wave, 1U);
_SFD_DATA_RANGE_CHECK(*c7_b_ampiezza, 2U);
_SFD_DATA_RANGE_CHECK(*c7_b_frequenza, 3U);
_SFD_DATA_RANGE_CHECK(*c7_b_noise, 4U);
_SFD_DATA_RANGE_CHECK(*c7_b_t, 5U);
chartInstance->c7_sfEvent = CALL_EVENT;
_SFD_CC_CALL(CHART_ENTER_DURING_FUNCTION_TAG, 6U, chartInstance->c7_sfEvent);
c7_hoistedGlobal = *c7_b_sfasamento;
c7_b_hoistedGlobal = *c7_b_ampiezza;
c7_c_hoistedGlobal = *c7_b_frequenza;
c7_d_hoistedGlobal = *c7_b_noise;
c7_e_hoistedGlobal = *c7_b_t;
c7_sfasamento = c7_hoistedGlobal;
c7_ampiezza = c7_b_hoistedGlobal;
c7_frequenza = c7_c_hoistedGlobal;
c7_noise = c7_d_hoistedGlobal;
c7_t = c7_e_hoistedGlobal;
sf_debug_symbol_scope_push_eml(0U, 8U, 8U, c7_debug_family_names,
c7_debug_family_var_map);
sf_debug_symbol_scope_add_eml_importable(&c7_nargin, 0U, c7_sf_marshallOut,
c7_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c7_nargout, 1U, c7_sf_marshallOut,
c7_sf_marshallIn);
sf_debug_symbol_scope_add_eml(&c7_sfasamento, 2U, c7_sf_marshallOut);
sf_debug_symbol_scope_add_eml(&c7_ampiezza, 3U, c7_sf_marshallOut);
sf_debug_symbol_scope_add_eml(&c7_frequenza, 4U, c7_sf_marshallOut);
sf_debug_symbol_scope_add_eml(&c7_noise, 5U, c7_sf_marshallOut);
sf_debug_symbol_scope_add_eml(&c7_t, 6U, c7_sf_marshallOut);
sf_debug_symbol_scope_add_eml_importable(&c7_wave, 7U, c7_sf_marshallOut,
c7_sf_marshallIn);
CV_EML_FCN(0, 0);
_SFD_EML_CALL(0U, chartInstance->c7_sfEvent, 4);
c7_b = c7_frequenza;
c7_y = 6.2831853071795862 * c7_b;
c7_a = c7_y;
c7_b_b = c7_t;
c7_b_y = c7_a * c7_b_b;
c7_x = c7_sfasamento + c7_b_y;
c7_b_x = c7_x;
c7_b_x = muDoubleScalarSin(c7_b_x);
c7_b_a = c7_ampiezza;
c7_c_b = c7_b_x;
c7_c_y = c7_b_a * c7_c_b;
c7_wave = c7_c_y + c7_noise;
_SFD_EML_CALL(0U, chartInstance->c7_sfEvent, -4);
sf_debug_symbol_scope_pop();
*c7_b_wave = c7_wave;
_SFD_CC_CALL(EXIT_OUT_OF_FUNCTION_TAG, 6U, chartInstance->c7_sfEvent);
sf_debug_check_for_state_inconsistency(_ekffedepreMachineNumber_,
chartInstance->chartNumber, chartInstance->instanceNumber);
}
static void c2_chartstep_c2_gesamtmodell(SFc2_gesamtmodellInstanceStruct
*chartInstance)
{
int32_T c2_i4;
real_T c2_u[4];
uint32_T c2_debug_family_var_map[9];
real_T c2_T;
real_T c2_phi;
real_T c2_theta;
real_T c2_psi;
real_T c2_Mt[9];
real_T c2_nargin = 1.0;
real_T c2_nargout = 1.0;
real_T c2_y[3];
real_T c2_x;
real_T c2_b_x;
real_T c2_c_x;
real_T c2_d_x;
real_T c2_a;
real_T c2_b;
real_T c2_b_y;
real_T c2_e_x;
real_T c2_f_x;
real_T c2_g_x;
real_T c2_h_x;
real_T c2_b_a;
real_T c2_b_b;
real_T c2_c_y;
real_T c2_i_x;
real_T c2_j_x;
real_T c2_c_a;
real_T c2_c_b;
real_T c2_d_y;
real_T c2_k_x;
real_T c2_l_x;
real_T c2_m_x;
real_T c2_n_x;
real_T c2_d_a;
real_T c2_d_b;
real_T c2_e_y;
real_T c2_o_x;
real_T c2_p_x;
real_T c2_q_x;
real_T c2_r_x;
real_T c2_e_a;
real_T c2_e_b;
real_T c2_f_y;
real_T c2_s_x;
real_T c2_t_x;
real_T c2_f_a;
real_T c2_f_b;
real_T c2_g_y;
real_T c2_u_x;
real_T c2_v_x;
real_T c2_w_x;
real_T c2_x_x;
real_T c2_g_a;
real_T c2_g_b;
real_T c2_h_y;
real_T c2_y_x;
real_T c2_ab_x;
real_T c2_bb_x;
real_T c2_cb_x;
real_T c2_h_a;
real_T c2_h_b;
real_T c2_i_y;
real_T c2_db_x;
real_T c2_eb_x;
real_T c2_fb_x;
real_T c2_gb_x;
real_T c2_i_a;
real_T c2_i_b;
real_T c2_j_y;
real_T c2_hb_x;
real_T c2_ib_x;
real_T c2_j_a;
real_T c2_j_b;
real_T c2_k_y;
real_T c2_jb_x;
real_T c2_kb_x;
real_T c2_lb_x;
real_T c2_mb_x;
real_T c2_k_a;
real_T c2_k_b;
real_T c2_l_y;
real_T c2_nb_x;
real_T c2_ob_x;
real_T c2_pb_x;
real_T c2_qb_x;
real_T c2_l_a;
real_T c2_l_b;
real_T c2_m_y;
real_T c2_rb_x;
real_T c2_sb_x;
real_T c2_m_a;
real_T c2_m_b;
real_T c2_n_y;
real_T c2_tb_x;
real_T c2_ub_x;
real_T c2_vb_x;
real_T c2_wb_x;
real_T c2_n_a;
real_T c2_n_b;
real_T c2_o_y;
real_T c2_xb_x;
real_T c2_yb_x;
real_T c2_ac_x;
real_T c2_bc_x;
real_T c2_cc_x;
real_T c2_dc_x;
real_T c2_o_a;
real_T c2_o_b;
real_T c2_p_y;
real_T c2_ec_x;
real_T c2_fc_x;
real_T c2_gc_x;
real_T c2_hc_x;
real_T c2_p_a;
real_T c2_p_b;
real_T c2_q_y;
int32_T c2_i5;
real_T c2_q_a[9];
real_T c2_q_b[3];
int32_T c2_i6;
int32_T c2_i7;
int32_T c2_i8;
real_T c2_C[3];
int32_T c2_i9;
int32_T c2_i10;
int32_T c2_i11;
int32_T c2_i12;
int32_T c2_i13;
int32_T c2_i14;
int32_T c2_i15;
real_T (*c2_r_y)[3];
real_T (*c2_b_u)[4];
c2_r_y = (real_T (*)[3])ssGetOutputPortSignal(chartInstance->S, 1);
c2_b_u = (real_T (*)[4])ssGetInputPortSignal(chartInstance->S, 0);
_SFD_CC_CALL(CHART_ENTER_DURING_FUNCTION_TAG, 1U, chartInstance->c2_sfEvent);
for (c2_i4 = 0; c2_i4 < 4; c2_i4++) {
c2_u[c2_i4] = (*c2_b_u)[c2_i4];
}
sf_debug_symbol_scope_push_eml(0U, 9U, 9U, c2_debug_family_names,
c2_debug_family_var_map);
sf_debug_symbol_scope_add_eml_importable(&c2_T, 0U, c2_c_sf_marshallOut,
c2_b_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c2_phi, 1U, c2_c_sf_marshallOut,
c2_b_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c2_theta, 2U, c2_c_sf_marshallOut,
c2_b_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c2_psi, 3U, c2_c_sf_marshallOut,
c2_b_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(c2_Mt, 4U, c2_d_sf_marshallOut,
c2_c_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c2_nargin, 5U, c2_c_sf_marshallOut,
c2_b_sf_marshallIn);
sf_debug_symbol_scope_add_eml_importable(&c2_nargout, 6U, c2_c_sf_marshallOut,
c2_b_sf_marshallIn);
sf_debug_symbol_scope_add_eml(c2_u, 7U, c2_b_sf_marshallOut);
sf_debug_symbol_scope_add_eml_importable(c2_y, 8U, c2_sf_marshallOut,
c2_sf_marshallIn);
CV_EML_FCN(0, 0);
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, 2);
c2_T = c2_u[0];
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, 3);
c2_phi = c2_u[1];
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, 4);
c2_theta = c2_u[2];
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, 5);
c2_psi = c2_u[3];
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, 7);
c2_x = c2_theta;
c2_b_x = c2_x;
c2_b_x = muDoubleScalarCos(c2_b_x);
c2_c_x = c2_psi;
c2_d_x = c2_c_x;
c2_d_x = muDoubleScalarCos(c2_d_x);
c2_a = c2_b_x;
c2_b = c2_d_x;
c2_b_y = c2_a * c2_b;
c2_e_x = c2_phi;
c2_f_x = c2_e_x;
c2_f_x = muDoubleScalarSin(c2_f_x);
c2_g_x = c2_theta;
c2_h_x = c2_g_x;
c2_h_x = muDoubleScalarSin(c2_h_x);
c2_b_a = c2_f_x;
c2_b_b = c2_h_x;
c2_c_y = c2_b_a * c2_b_b;
c2_i_x = c2_psi;
c2_j_x = c2_i_x;
c2_j_x = muDoubleScalarCos(c2_j_x);
c2_c_a = c2_c_y;
c2_c_b = c2_j_x;
c2_d_y = c2_c_a * c2_c_b;
c2_k_x = c2_phi;
c2_l_x = c2_k_x;
c2_l_x = muDoubleScalarCos(c2_l_x);
c2_m_x = c2_psi;
c2_n_x = c2_m_x;
c2_n_x = muDoubleScalarSin(c2_n_x);
c2_d_a = c2_l_x;
c2_d_b = c2_n_x;
c2_e_y = c2_d_a * c2_d_b;
c2_o_x = c2_phi;
c2_p_x = c2_o_x;
c2_p_x = muDoubleScalarCos(c2_p_x);
c2_q_x = c2_theta;
c2_r_x = c2_q_x;
c2_r_x = muDoubleScalarSin(c2_r_x);
c2_e_a = c2_p_x;
c2_e_b = c2_r_x;
c2_f_y = c2_e_a * c2_e_b;
c2_s_x = c2_psi;
c2_t_x = c2_s_x;
c2_t_x = muDoubleScalarCos(c2_t_x);
c2_f_a = c2_f_y;
c2_f_b = c2_t_x;
c2_g_y = c2_f_a * c2_f_b;
c2_u_x = c2_phi;
c2_v_x = c2_u_x;
c2_v_x = muDoubleScalarSin(c2_v_x);
c2_w_x = c2_psi;
c2_x_x = c2_w_x;
c2_x_x = muDoubleScalarSin(c2_x_x);
c2_g_a = c2_v_x;
c2_g_b = c2_x_x;
c2_h_y = c2_g_a * c2_g_b;
c2_y_x = c2_theta;
c2_ab_x = c2_y_x;
c2_ab_x = muDoubleScalarCos(c2_ab_x);
c2_bb_x = c2_psi;
c2_cb_x = c2_bb_x;
c2_cb_x = muDoubleScalarSin(c2_cb_x);
c2_h_a = c2_ab_x;
c2_h_b = c2_cb_x;
c2_i_y = c2_h_a * c2_h_b;
c2_db_x = c2_phi;
c2_eb_x = c2_db_x;
c2_eb_x = muDoubleScalarSin(c2_eb_x);
c2_fb_x = c2_theta;
c2_gb_x = c2_fb_x;
c2_gb_x = muDoubleScalarSin(c2_gb_x);
c2_i_a = c2_eb_x;
c2_i_b = c2_gb_x;
c2_j_y = c2_i_a * c2_i_b;
c2_hb_x = c2_psi;
c2_ib_x = c2_hb_x;
c2_ib_x = muDoubleScalarSin(c2_ib_x);
c2_j_a = c2_j_y;
c2_j_b = c2_ib_x;
c2_k_y = c2_j_a * c2_j_b;
c2_jb_x = c2_phi;
c2_kb_x = c2_jb_x;
c2_kb_x = muDoubleScalarCos(c2_kb_x);
c2_lb_x = c2_psi;
c2_mb_x = c2_lb_x;
c2_mb_x = muDoubleScalarCos(c2_mb_x);
c2_k_a = c2_kb_x;
c2_k_b = c2_mb_x;
c2_l_y = c2_k_a * c2_k_b;
c2_nb_x = c2_phi;
c2_ob_x = c2_nb_x;
c2_ob_x = muDoubleScalarCos(c2_ob_x);
c2_pb_x = c2_theta;
c2_qb_x = c2_pb_x;
c2_qb_x = muDoubleScalarSin(c2_qb_x);
c2_l_a = c2_ob_x;
c2_l_b = c2_qb_x;
c2_m_y = c2_l_a * c2_l_b;
c2_rb_x = c2_psi;
c2_sb_x = c2_rb_x;
c2_sb_x = muDoubleScalarSin(c2_sb_x);
c2_m_a = c2_m_y;
c2_m_b = c2_sb_x;
c2_n_y = c2_m_a * c2_m_b;
c2_tb_x = c2_phi;
c2_ub_x = c2_tb_x;
c2_ub_x = muDoubleScalarSin(c2_ub_x);
c2_vb_x = c2_psi;
c2_wb_x = c2_vb_x;
c2_wb_x = muDoubleScalarCos(c2_wb_x);
c2_n_a = c2_ub_x;
c2_n_b = c2_wb_x;
c2_o_y = c2_n_a * c2_n_b;
c2_xb_x = c2_theta;
c2_yb_x = c2_xb_x;
c2_yb_x = muDoubleScalarSin(c2_yb_x);
c2_ac_x = c2_phi;
c2_bc_x = c2_ac_x;
c2_bc_x = muDoubleScalarSin(c2_bc_x);
c2_cc_x = c2_theta;
c2_dc_x = c2_cc_x;
c2_dc_x = muDoubleScalarCos(c2_dc_x);
c2_o_a = c2_bc_x;
c2_o_b = c2_dc_x;
c2_p_y = c2_o_a * c2_o_b;
c2_ec_x = c2_phi;
c2_fc_x = c2_ec_x;
c2_fc_x = muDoubleScalarCos(c2_fc_x);
c2_gc_x = c2_theta;
c2_hc_x = c2_gc_x;
c2_hc_x = muDoubleScalarCos(c2_hc_x);
c2_p_a = c2_fc_x;
c2_p_b = c2_hc_x;
c2_q_y = c2_p_a * c2_p_b;
c2_Mt[0] = c2_b_y;
c2_Mt[3] = c2_d_y - c2_e_y;
c2_Mt[6] = c2_g_y + c2_h_y;
c2_Mt[1] = c2_i_y;
c2_Mt[4] = c2_k_y + c2_l_y;
c2_Mt[7] = c2_n_y - c2_o_y;
c2_Mt[2] = -c2_yb_x;
c2_Mt[5] = c2_p_y;
c2_Mt[8] = c2_q_y;
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, 10);
for (c2_i5 = 0; c2_i5 < 9; c2_i5++) {
c2_q_a[c2_i5] = c2_Mt[c2_i5];
}
c2_q_b[0] = 0.0;
c2_q_b[1] = 0.0;
c2_q_b[2] = c2_T;
c2_eml_scalar_eg(chartInstance);
c2_eml_scalar_eg(chartInstance);
for (c2_i6 = 0; c2_i6 < 3; c2_i6++) {
c2_y[c2_i6] = 0.0;
}
for (c2_i7 = 0; c2_i7 < 3; c2_i7++) {
c2_y[c2_i7] = 0.0;
}
for (c2_i8 = 0; c2_i8 < 3; c2_i8++) {
c2_C[c2_i8] = c2_y[c2_i8];
}
for (c2_i9 = 0; c2_i9 < 3; c2_i9++) {
c2_y[c2_i9] = c2_C[c2_i9];
}
for (c2_i10 = 0; c2_i10 < 3; c2_i10++) {
c2_C[c2_i10] = c2_y[c2_i10];
}
for (c2_i11 = 0; c2_i11 < 3; c2_i11++) {
c2_y[c2_i11] = c2_C[c2_i11];
}
for (c2_i12 = 0; c2_i12 < 3; c2_i12++) {
c2_y[c2_i12] = 0.0;
c2_i13 = 0;
for (c2_i14 = 0; c2_i14 < 3; c2_i14++) {
c2_y[c2_i12] += c2_q_a[c2_i13 + c2_i12] * c2_q_b[c2_i14];
c2_i13 += 3;
}
}
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, -10);
sf_debug_symbol_scope_pop();
for (c2_i15 = 0; c2_i15 < 3; c2_i15++) {
(*c2_r_y)[c2_i15] = c2_y[c2_i15];
}
_SFD_CC_CALL(EXIT_OUT_OF_FUNCTION_TAG, 1U, chartInstance->c2_sfEvent);
}