static void sf_c2_MigrationOWBG_Proto4_1DLinear2DNonLinear
(SFc2_MigrationOWBG_Proto4_1DLinear2DNonLinearInstanceStruct *chartInstance)
{
real_T c2_hoistedGlobal;
real_T c2_b_hoistedGlobal;
real_T c2_x;
real_T c2_y;
uint32_T c2_debug_family_var_map[5];
real_T c2_nargin = 2.0;
real_T c2_nargout = 1.0;
real_T c2_z;
real_T c2_b_x;
real_T c2_c_x;
real_T *c2_d_x;
real_T *c2_b_y;
real_T *c2_b_z;
c2_b_z = (real_T *)ssGetOutputPortSignal(chartInstance->S, 1);
c2_b_y = (real_T *)ssGetInputPortSignal(chartInstance->S, 1);
c2_d_x = (real_T *)ssGetInputPortSignal(chartInstance->S, 0);
_sfTime_ = (real_T)ssGetT(chartInstance->S);
_SFD_CC_CALL(CHART_ENTER_SFUNCTION_TAG, 0U, chartInstance->c2_sfEvent);
_SFD_DATA_RANGE_CHECK(*c2_d_x, 0U);
_SFD_DATA_RANGE_CHECK(*c2_b_y, 1U);
_SFD_DATA_RANGE_CHECK(*c2_b_z, 2U);
chartInstance->c2_sfEvent = CALL_EVENT;
_SFD_CC_CALL(CHART_ENTER_DURING_FUNCTION_TAG, 0U, chartInstance->c2_sfEvent);
c2_hoistedGlobal = *c2_d_x;
c2_b_hoistedGlobal = *c2_b_y;
c2_x = c2_hoistedGlobal;
c2_y = c2_b_hoistedGlobal;
_SFD_SYMBOL_SCOPE_PUSH_EML(0U, 5U, 5U, c2_debug_family_names,
c2_debug_family_var_map);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c2_nargin, 0U, c2_sf_marshallOut,
c2_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c2_nargout, 1U, c2_sf_marshallOut,
c2_sf_marshallIn);
_SFD_SYMBOL_SCOPE_ADD_EML(&c2_x, 2U, c2_sf_marshallOut);
_SFD_SYMBOL_SCOPE_ADD_EML(&c2_y, 3U, c2_sf_marshallOut);
_SFD_SYMBOL_SCOPE_ADD_EML_IMPORTABLE(&c2_z, 4U, c2_sf_marshallOut,
c2_sf_marshallIn);
CV_EML_FCN(0, 0);
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, 3);
c2_b_x = (45.96 - c2_mpower(chartInstance, c2_y)) - c2_mpower(chartInstance,
c2_x);
c2_z = c2_b_x;
if (c2_z < 0.0) {
c2_eml_error(chartInstance);
}
c2_c_x = c2_z;
c2_z = c2_c_x;
c2_z = muDoubleScalarSqrt(c2_z);
_SFD_EML_CALL(0U, chartInstance->c2_sfEvent, -3);
_SFD_SYMBOL_SCOPE_POP();
*c2_b_z = c2_z;
_SFD_CC_CALL(EXIT_OUT_OF_FUNCTION_TAG, 0U, chartInstance->c2_sfEvent);
_SFD_CHECK_FOR_STATE_INCONSISTENCY
(_MigrationOWBG_Proto4_1DLinear2DNonLinearMachineNumber_,
chartInstance->chartNumber, chartInstance->instanceNumber);
}
static void c2_c2_MPC_framework(void)
{
real_T c2_b_x;
real_T c2_b_y;
real_T c2_b_al1_c;
real_T c2_b_al2_c;
real_T c2_b_a1;
real_T c2_b_a2;
real_T c2_al2_2;
real_T c2_al1_al2_2;
real_T c2_sin_al1_al2_2;
real_T c2_cos_al1_al2_2;
real_T c2_al2_1;
real_T c2_al1_al2_1;
real_T c2_sin_al1_al2_1;
real_T c2_cos_al1_al2_1;
real_T c2_al2_n;
real_T c2_al2_p;
real_T c2_al1_2;
real_T c2_al1_1;
real_T c2_cos_al1_n;
real_T c2_cos_al1_p;
real_T c2_sin_al1_n;
real_T c2_sin_al1_p;
real_T c2_D;
real_T c2_cos_al2;
real_T c2_d;
uint8_T c2_b_error;
real_T c2_b_al2;
real_T c2_b_al1;
real_T c2_c_x;
real_T c2_c_y;
boolean_T c2_em_b0;
boolean_T c2_em_b1;
real_T c2_A;
real_T c2_B;
real_T c2_d_x;
real_T c2_d_y;
real_T c2_z;
real_T c2_e_y;
real_T c2_e_x;
real_T c2_f_y;
real_T c2_b_A;
real_T c2_b_B;
real_T c2_f_x;
real_T c2_g_y;
real_T c2_b_z;
real_T c2_h_y;
real_T c2_g_x;
real_T c2_i_y;
real_T c2_c_A;
real_T c2_c_B;
real_T c2_h_x;
real_T c2_j_y;
real_T c2_c_z;
real_T c2_k_y;
real_T c2_i_x;
real_T c2_l_y;
real_T c2_d_A;
real_T c2_d_B;
real_T c2_j_x;
real_T c2_m_y;
real_T c2_d_z;
real_T c2_n_y;
real_T c2_k_x;
real_T c2_o_y;
real_T c2_e_A;
real_T c2_e_B;
real_T c2_l_x;
real_T c2_p_y;
real_T c2_e_z;
real_T c2_q_y;
real_T c2_m_x;
real_T c2_k;
real_T c2_b_k;
real_T c2_r_y;
real_T c2_n_x;
real_T c2_s_y;
real_T c2_f_A;
real_T c2_f_B;
real_T c2_o_x;
real_T c2_t_y;
real_T c2_f_z;
real_T c2_u_y;
real_T c2_p_x;
real_T c2_v_y;
real_T c2_g_A;
real_T c2_g_B;
real_T c2_q_x;
real_T c2_w_y;
real_T c2_g_z;
real_T c2_x_y;
real_T c2_r_x;
real_T c2_y_y;
real_T c2_h_A;
real_T c2_h_B;
real_T c2_s_x;
real_T c2_ab_y;
real_T c2_h_z;
real_T c2_bb_y;
real_T c2_t_x;
real_T c2_cb_y;
real_T c2_i_A;
real_T c2_i_B;
real_T c2_u_x;
real_T c2_db_y;
real_T c2_i_z;
real_T c2_eb_y;
real_T c2_v_x;
real_T c2_fb_y;
real_T c2_w_x;
real_T c2_gb_y;
_SFD_CC_CALL(CHART_ENTER_DURING_FUNCTION_TAG,1);
c2_b_x = *c2_x();
c2_b_y = *c2_y();
c2_b_al1_c = *c2_al1_c();
c2_b_al2_c = *c2_al2_c();
c2_b_a1 = *c2_a1();
c2_b_a2 = *c2_a2();
sf_debug_push_symbol_scope(28U, 0U);
sf_debug_symbol_scope_add_symbol("al2_2", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0, 0U,
0, 0U, 0, &c2_al2_2, 0);
sf_debug_symbol_scope_add_symbol("al1_al2_2", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0,
0U, 0, 0U, 0, &c2_al1_al2_2, 0);
sf_debug_symbol_scope_add_symbol("sin_al1_al2_2", 0, 0U, 0U, 0U, 0U, 1.0, 0,
0.0, 0U, 0, 0U, 0, &c2_sin_al1_al2_2, 0);
sf_debug_symbol_scope_add_symbol("cos_al1_al2_2", 0, 0U, 0U, 0U, 0U, 1.0, 0,
0.0, 0U, 0, 0U, 0, &c2_cos_al1_al2_2, 0);
sf_debug_symbol_scope_add_symbol("al2_1", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0, 0U,
0, 0U, 0, &c2_al2_1, 0);
sf_debug_symbol_scope_add_symbol("al1_al2_1", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0,
0U, 0, 0U, 0, &c2_al1_al2_1, 0);
sf_debug_symbol_scope_add_symbol("sin_al1_al2_1", 0, 0U, 0U, 0U, 0U, 1.0, 0,
0.0, 0U, 0, 0U, 0, &c2_sin_al1_al2_1, 0);
sf_debug_symbol_scope_add_symbol("cos_al1_al2_1", 0, 0U, 0U, 0U, 0U, 1.0, 0,
0.0, 0U, 0, 0U, 0, &c2_cos_al1_al2_1, 0);
sf_debug_symbol_scope_add_symbol("al2_n", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0, 0U,
0, 0U, 0, &c2_al2_n, 0);
sf_debug_symbol_scope_add_symbol("al2_p", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0, 0U,
0, 0U, 0, &c2_al2_p, 0);
sf_debug_symbol_scope_add_symbol("al1_2", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0, 0U,
0, 0U, 0, &c2_al1_2, 0);
sf_debug_symbol_scope_add_symbol("al1_1", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0, 0U,
0, 0U, 0, &c2_al1_1, 0);
sf_debug_symbol_scope_add_symbol("cos_al1_n", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0,
0U, 0, 0U, 0, &c2_cos_al1_n, 0);
sf_debug_symbol_scope_add_symbol("cos_al1_p", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0,
0U, 0, 0U, 0, &c2_cos_al1_p, 0);
sf_debug_symbol_scope_add_symbol("sin_al1_n", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0,
0U, 0, 0U, 0, &c2_sin_al1_n, 0);
sf_debug_symbol_scope_add_symbol("sin_al1_p", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0,
0U, 0, 0U, 0, &c2_sin_al1_p, 0);
sf_debug_symbol_scope_add_symbol("D", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0, 0U, 0,
0U, 0, &c2_D, 0);
sf_debug_symbol_scope_add_symbol("cos_al2", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0,
0U, 0, 0U, 0, &c2_cos_al2, 0);
sf_debug_symbol_scope_add_symbol("d", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0, 0U, 0,
0U, 0, &c2_d, 0);
sf_debug_symbol_scope_add_symbol("error", 3, 0U, 0U, 0U, 0U, 1.0, 0, 0.0, 0U,
0, 0U, 0, &c2_b_error, 0);
sf_debug_symbol_scope_add_symbol("al2", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0, 0U, 0,
0U, 0, &c2_b_al2, 0);
sf_debug_symbol_scope_add_symbol("al1", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0, 0U, 0,
0U, 0, &c2_b_al1, 0);
sf_debug_symbol_scope_add_symbol("a2", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0, 0U, 0,
0U, 0, &c2_b_a2, 0);
sf_debug_symbol_scope_add_symbol("a1", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0, 0U, 0,
0U, 0, &c2_b_a1, 0);
sf_debug_symbol_scope_add_symbol("al2_c", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0, 0U,
0, 0U, 0, &c2_b_al2_c, 0);
sf_debug_symbol_scope_add_symbol("al1_c", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0, 0U,
0, 0U, 0, &c2_b_al1_c, 0);
sf_debug_symbol_scope_add_symbol("y", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0, 0U, 0,
0U, 0, &c2_b_y, 0);
sf_debug_symbol_scope_add_symbol("x", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0, 0U, 0,
0U, 0, &c2_b_x, 0);
CV_EML_FCN(0, 0);
_SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,3);
c2_c_x = c2_mpower(c2_b_x) + c2_mpower(c2_b_y);
c2_c_y = sqrt(c2_c_x);
c2_d = c2_c_y;
_SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,4);
c2_em_b0 = (c2_mpower(c2_d) > c2_mpower(c2_b_a1) + c2_mpower(c2_b_a2));
c2_em_b1 = (c2_d < c2_b_a1 - c2_b_a2);
if(CV_EML_IF(0, 0, c2_em_b0 || c2_em_b1)) {
_SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,5);
c2_b_al1 = c2_b_al1_c;
_SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,6);
c2_b_al2 = c2_b_al2_c;
_SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,7);
c2_b_error = 1U;
} else {
_SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,11);
c2_A = ((c2_mpower(c2_b_x) + c2_mpower(c2_b_y)) - c2_mpower(c2_b_a1)) -
c2_mpower(c2_b_a2);
c2_B = 2.0 * c2_b_a1 * c2_b_a2;
c2_d_x = c2_A;
c2_d_y = c2_B;
c2_z = c2_d_x / c2_d_y;
c2_e_y = c2_z;
c2_cos_al2 = c2_e_y;
_SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,12);
c2_D = c2_b_a1 + c2_b_a2 * c2_cos_al2;
_SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,14);
c2_e_x = (c2_mpower(c2_b_x) + c2_mpower(c2_b_y)) - c2_mpower(c2_D);
c2_f_y = sqrt(c2_e_x);
c2_b_A = c2_D * c2_b_y + c2_b_x * c2_f_y;
c2_b_B = c2_mpower(c2_b_x) + c2_mpower(c2_b_y);
c2_f_x = c2_b_A;
c2_g_y = c2_b_B;
c2_b_z = c2_f_x / c2_g_y;
c2_h_y = c2_b_z;
c2_sin_al1_p = c2_h_y;
_SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,15);
c2_g_x = (c2_mpower(c2_b_x) + c2_mpower(c2_b_y)) - c2_mpower(c2_D);
c2_i_y = sqrt(c2_g_x);
c2_c_A = c2_D * c2_b_y - c2_b_x * c2_i_y;
c2_c_B = c2_mpower(c2_b_x) + c2_mpower(c2_b_y);
c2_h_x = c2_c_A;
c2_j_y = c2_c_B;
c2_c_z = c2_h_x / c2_j_y;
c2_k_y = c2_c_z;
c2_sin_al1_n = c2_k_y;
_SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,17);
c2_i_x = (c2_mpower(c2_b_x) + c2_mpower(c2_b_y)) - c2_mpower(c2_D);
c2_l_y = sqrt(c2_i_x);
c2_d_A = c2_D * c2_b_x + c2_b_y * c2_l_y;
c2_d_B = c2_mpower(c2_b_x) + c2_mpower(c2_b_y);
c2_j_x = c2_d_A;
c2_m_y = c2_d_B;
c2_d_z = c2_j_x / c2_m_y;
c2_n_y = c2_d_z;
c2_cos_al1_p = c2_n_y;
_SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,18);
c2_k_x = (c2_mpower(c2_b_x) + c2_mpower(c2_b_y)) - c2_mpower(c2_D);
c2_o_y = sqrt(c2_k_x);
c2_e_A = c2_D * c2_b_x - c2_b_y * c2_o_y;
c2_e_B = c2_mpower(c2_b_x) + c2_mpower(c2_b_y);
c2_l_x = c2_e_A;
c2_p_y = c2_e_B;
c2_e_z = c2_l_x / c2_p_y;
c2_q_y = c2_e_z;
c2_cos_al1_n = c2_q_y;
_SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,20);
c2_al1_1 = c2_atan2(c2_sin_al1_p, c2_cos_al1_n);
_SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,22);
c2_al1_2 = c2_atan2(c2_sin_al1_n, c2_cos_al1_p);
_SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,24);
c2_m_x = c2_cos_al2;
c2_k = 1.0;
c2_b_k = c2_k;
_SFD_EML_ARRAY_BOUNDS_CHECK("x", (int32_T)_SFD_INTEGER_CHECK("k", c2_b_k),
1, 1, 1);
if(c2_m_x < -1.0) {
} else {
_SFD_EML_ARRAY_BOUNDS_CHECK("x", (int32_T)_SFD_INTEGER_CHECK("k", c2_b_k),
1, 1, 1);
if(c2_m_x > 1.0) {
} else {
goto label_1;
}
}
sf_mex_call("error", 0U, 1U, 15,
"Domain error. To compute complex results from real x, use \'acos(complex(x))\'.");
label_1:;
c2_r_y = acos(c2_m_x);
c2_al2_p = c2_r_y;
_SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,25);
c2_al2_n = -c2_al2_p;
_SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,27);
c2_n_x = c2_al1_1;
c2_s_y = cos(c2_n_x);
c2_f_A = c2_b_x - c2_b_a1 * c2_s_y;
c2_f_B = c2_b_a2;
c2_o_x = c2_f_A;
c2_t_y = c2_f_B;
c2_f_z = c2_o_x / c2_t_y;
c2_u_y = c2_f_z;
c2_cos_al1_al2_1 = c2_u_y;
_SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,28);
c2_p_x = c2_al1_1;
c2_v_y = sin(c2_p_x);
c2_g_A = c2_b_y - c2_b_a1 * c2_v_y;
c2_g_B = c2_b_a2;
c2_q_x = c2_g_A;
c2_w_y = c2_g_B;
c2_g_z = c2_q_x / c2_w_y;
c2_x_y = c2_g_z;
c2_sin_al1_al2_1 = c2_x_y;
_SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,29);
c2_al1_al2_1 = c2_atan2(c2_sin_al1_al2_1, c2_cos_al1_al2_1);
_SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,30);
c2_al2_1 = c2_al1_al2_1 - c2_al1_1;
_SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,32);
c2_r_x = c2_al1_2;
c2_y_y = cos(c2_r_x);
c2_h_A = c2_b_x - c2_b_a1 * c2_y_y;
c2_h_B = c2_b_a2;
c2_s_x = c2_h_A;
c2_ab_y = c2_h_B;
c2_h_z = c2_s_x / c2_ab_y;
c2_bb_y = c2_h_z;
c2_cos_al1_al2_2 = c2_bb_y;
_SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,33);
c2_t_x = c2_al1_2;
c2_cb_y = sin(c2_t_x);
c2_i_A = c2_b_y - c2_b_a1 * c2_cb_y;
c2_i_B = c2_b_a2;
c2_u_x = c2_i_A;
c2_db_y = c2_i_B;
c2_i_z = c2_u_x / c2_db_y;
c2_eb_y = c2_i_z;
c2_sin_al1_al2_2 = c2_eb_y;
_SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,34);
c2_al1_al2_2 = c2_atan2(c2_sin_al1_al2_2, c2_cos_al1_al2_2);
_SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,35);
c2_al2_2 = c2_al1_al2_2 - c2_al1_2;
_SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,37);
c2_v_x = c2_b_al1_c - c2_al1_1;
c2_fb_y = fabs(c2_v_x);
c2_w_x = c2_b_al1_c - c2_al1_2;
c2_gb_y = fabs(c2_w_x);
if(CV_EML_IF(0, 1, c2_fb_y < c2_gb_y)) {
_SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,38);
c2_b_al1 = c2_al1_1;
_SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,39);
c2_b_al2 = c2_al2_1;
} else {
_SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,41);
c2_b_al1 = c2_al1_2;
_SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,42);
c2_b_al2 = c2_al2_2;
}
_SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,45);
c2_b_error = 0U;
}
_SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,-45);
sf_debug_pop_symbol_scope();
*c2_al1() = c2_b_al1;
*c2_al2() = c2_b_al2;
*c2_error() = c2_b_error;
_SFD_CC_CALL(EXIT_OUT_OF_FUNCTION_TAG,1);
}
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);
}