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);
}
/* Function Definitions */
boolean_T relop(const creal_T a, const creal_T b)
{
boolean_T p;
real_T absbi;
real_T y;
real_T absxk;
int32_T exponent;
real_T absar;
real_T absbr;
real_T Ma;
int32_T b_exponent;
int32_T c_exponent;
int32_T d_exponent;
if ((muDoubleScalarAbs(a.re) > 8.9884656743115785E+307) || (muDoubleScalarAbs
(a.im) > 8.9884656743115785E+307) || (muDoubleScalarAbs(b.re) >
8.9884656743115785E+307) || (muDoubleScalarAbs(b.im) >
8.9884656743115785E+307)) {
absbi = muDoubleScalarHypot(a.re / 2.0, a.im / 2.0);
y = muDoubleScalarHypot(b.re / 2.0, b.im / 2.0);
} else {
absbi = muDoubleScalarHypot(a.re, a.im);
y = muDoubleScalarHypot(b.re, b.im);
}
absxk = y / 2.0;
if ((!muDoubleScalarIsInf(absxk)) && (!muDoubleScalarIsNaN(absxk))) {
if (absxk <= 2.2250738585072014E-308) {
absxk = 4.94065645841247E-324;
} else {
frexp(absxk, &exponent);
absxk = ldexp(1.0, exponent - 53);
}
} else {
absxk = rtNaN;
}
if ((muDoubleScalarAbs(y - absbi) < absxk) || (muDoubleScalarIsInf(absbi) &&
muDoubleScalarIsInf(y) && ((absbi > 0.0) == (y > 0.0)))) {
p = true;
} else {
p = false;
}
if (p) {
absar = muDoubleScalarAbs(a.re);
absxk = muDoubleScalarAbs(a.im);
absbr = muDoubleScalarAbs(b.re);
absbi = muDoubleScalarAbs(b.im);
if (absar > absxk) {
Ma = absar;
absar = absxk;
} else {
Ma = absxk;
}
if (absbr > absbi) {
absxk = absbr;
absbr = absbi;
} else {
absxk = absbi;
}
if (Ma > absxk) {
if (absar < absbr) {
absbi = Ma - absxk;
y = (absar / 2.0 + absbr / 2.0) / (Ma / 2.0 + absxk / 2.0) * (absbr -
absar);
} else {
absbi = Ma;
y = absxk;
}
} else if (Ma < absxk) {
if (absar > absbr) {
y = absxk - Ma;
absbi = (absar / 2.0 + absbr / 2.0) / (Ma / 2.0 + absxk / 2.0) * (absar
- absbr);
} else {
absbi = Ma;
y = absxk;
}
} else {
absbi = absar;
y = absbr;
}
absxk = muDoubleScalarAbs(y / 2.0);
if ((!muDoubleScalarIsInf(absxk)) && (!muDoubleScalarIsNaN(absxk))) {
if (absxk <= 2.2250738585072014E-308) {
absxk = 4.94065645841247E-324;
} else {
frexp(absxk, &b_exponent);
absxk = ldexp(1.0, b_exponent - 53);
}
} else {
absxk = rtNaN;
}
if ((muDoubleScalarAbs(y - absbi) < absxk) || (muDoubleScalarIsInf(absbi) &&
muDoubleScalarIsInf(y) && ((absbi > 0.0) == (y > 0.0)))) {
p = true;
} else {
p = false;
}
if (p) {
absbi = muDoubleScalarAtan2(a.im, a.re);
y = muDoubleScalarAtan2(b.im, b.re);
absxk = muDoubleScalarAbs(y / 2.0);
if ((!muDoubleScalarIsInf(absxk)) && (!muDoubleScalarIsNaN(absxk))) {
if (absxk <= 2.2250738585072014E-308) {
absxk = 4.94065645841247E-324;
} else {
frexp(absxk, &c_exponent);
absxk = ldexp(1.0, c_exponent - 53);
}
} else {
absxk = rtNaN;
}
if ((muDoubleScalarAbs(y - absbi) < absxk) || (muDoubleScalarIsInf(absbi) &&
muDoubleScalarIsInf(y) && ((absbi > 0.0) == (y > 0.0)))) {
p = true;
} else {
p = false;
}
if (p) {
if (absbi > 0.78539816339744828) {
if (absbi > 2.3561944901923448) {
absbi = -a.im;
y = -b.im;
} else {
absbi = -a.re;
y = -b.re;
}
} else if (absbi > -0.78539816339744828) {
absbi = a.im;
y = b.im;
} else if (absbi > -2.3561944901923448) {
absbi = a.re;
y = b.re;
} else {
absbi = -a.im;
y = -b.im;
}
absxk = muDoubleScalarAbs(y / 2.0);
if ((!muDoubleScalarIsInf(absxk)) && (!muDoubleScalarIsNaN(absxk))) {
if (absxk <= 2.2250738585072014E-308) {
absxk = 4.94065645841247E-324;
} else {
frexp(absxk, &d_exponent);
absxk = ldexp(1.0, d_exponent - 53);
}
} else {
absxk = rtNaN;
}
if ((muDoubleScalarAbs(y - absbi) < absxk) || (muDoubleScalarIsInf(absbi)
&& muDoubleScalarIsInf(y) && ((absbi > 0.0) == (y > 0.0)))) {
p = true;
} else {
p = false;
}
if (p) {
absbi = 0.0;
y = 0.0;
}
}
}
}
return absbi <= y;
}
/* Function Definitions */
real_T angle(const creal_T x)
{
return muDoubleScalarAtan2(x.im, x.re);
}
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);
}
