diff --git a/hdw-a339x/src/wasm/fbw_a320/src/model/AutopilotLaws.cpp b/hdw-a339x/src/wasm/fbw_a320/src/model/AutopilotLaws.cpp new file mode 100644 index 000000000..acb52dced --- /dev/null +++ b/hdw-a339x/src/wasm/fbw_a320/src/model/AutopilotLaws.cpp @@ -0,0 +1,2561 @@ +#include "AutopilotLaws.h" +#include "rtwtypes.h" +#include "AutopilotLaws_types.h" +#include +#include "mod_HV94nWDg.h" +#include "rt_modd.h" +#include "look1_binlxpw.h" + +const uint8_T AutopilotLaws_IN_any{ 1U }; + +const uint8_T AutopilotLaws_IN_left{ 2U }; + +const uint8_T AutopilotLaws_IN_right{ 3U }; + +const uint8_T AutopilotLaws_IN_any_n{ 1U }; + +const uint8_T AutopilotLaws_IN_left_l{ 2U }; + +const uint8_T AutopilotLaws_IN_right_j{ 3U }; + +const uint8_T AutopilotLaws_IN_InAir{ 1U }; + +const uint8_T AutopilotLaws_IN_OnGround{ 2U }; + +void AutopilotLawsModelClass::AutopilotLaws_MATLABFunction(real_T rtu_tau, real_T rtu_zeta, real_T *rty_k2, real_T + *rty_k1) +{ + real_T t; + t = rtu_tau / 3600.0; + *rty_k1 = 180.0 / (39.478417604357432 * rtu_zeta * t); + *rty_k2 = rtu_zeta / (215666.565757755 * t); +} + +void AutopilotLawsModelClass::AutopilotLaws_LagFilter(real_T rtu_U, real_T rtu_C1, real_T rtu_dt, real_T *rty_Y, + rtDW_LagFilter_AutopilotLaws_T *localDW) +{ + real_T ca; + real_T denom_tmp; + if ((!localDW->pY_not_empty) || (!localDW->pU_not_empty)) { + localDW->pU = rtu_U; + localDW->pU_not_empty = true; + localDW->pY = rtu_U; + localDW->pY_not_empty = true; + } + + denom_tmp = rtu_dt * rtu_C1; + ca = denom_tmp / (denom_tmp + 2.0); + *rty_Y = (2.0 - denom_tmp) / (denom_tmp + 2.0) * localDW->pY + (rtu_U * ca + localDW->pU * ca); + localDW->pY = *rty_Y; + localDW->pU = rtu_U; +} + +void AutopilotLawsModelClass::AutopilotLaws_RateLimiter(real_T rtu_u, real_T rtu_up, real_T rtu_lo, real_T rtu_Ts, + real_T rtu_init, real_T *rty_Y, rtDW_RateLimiter_AutopilotLaws_T *localDW) +{ + if (!localDW->pY_not_empty) { + localDW->pY = rtu_init; + localDW->pY_not_empty = true; + } + + localDW->pY += std::fmax(std::fmin(rtu_u - localDW->pY, std::abs(rtu_up) * rtu_Ts), -std::abs(rtu_lo) * rtu_Ts); + *rty_Y = localDW->pY; +} + +void AutopilotLawsModelClass::AutopilotLaws_MATLABFunction_f_Init(rtDW_MATLABFunction_AutopilotLaws_d_T *localDW) +{ + localDW->limit = 30.0; +} + +void AutopilotLawsModelClass::AutopilotLaws_MATLABFunction_m(real_T rtu_Psi_c, real_T rtu_dPsi, real_T rtu_Phi_c, real_T + *rty_up, real_T *rty_lo, rtDW_MATLABFunction_AutopilotLaws_d_T *localDW) +{ + static const int8_T b[5]{ 0, 5, 10, 20, 30 }; + + static const int8_T c[5]{ 5, 5, 10, 30, 30 }; + + boolean_T wasPsiCmdChanged; + if (!localDW->lastPsi_not_empty) { + localDW->lastPsi = rtu_Psi_c; + localDW->lastPsi_not_empty = true; + } + + wasPsiCmdChanged = (rtu_Psi_c != localDW->lastPsi); + if (wasPsiCmdChanged || (std::abs(rtu_dPsi) > localDW->limitDeltaPsi)) { + localDW->limitDeltaPsi = std::abs(rtu_dPsi); + if (localDW->limitDeltaPsi > 30.0) { + localDW->limit = 30.0; + } else { + real_T r; + int32_T high_i; + int32_T low_i; + int32_T low_ip1; + high_i = 5; + low_i = 0; + low_ip1 = 2; + while (high_i > low_ip1) { + int32_T mid_i; + mid_i = ((low_i + high_i) + 1) >> 1; + if (localDW->limitDeltaPsi >= b[mid_i - 1]) { + low_i = mid_i - 1; + low_ip1 = mid_i + 1; + } else { + high_i = mid_i; + } + } + + r = (localDW->limitDeltaPsi - static_cast(b[low_i])) / static_cast(b[low_i + 1] - b[low_i]); + if (r == 0.0) { + localDW->limit = c[low_i]; + } else if (r == 1.0) { + localDW->limit = c[low_i + 1]; + } else if (c[low_i + 1] == c[low_i]) { + localDW->limit = c[low_i]; + } else { + localDW->limit = (1.0 - r) * static_cast(c[low_i]) + static_cast(c[low_i + 1]) * r; + } + } + } + + if ((!wasPsiCmdChanged) && (std::abs(rtu_Phi_c) < localDW->limit)) { + localDW->limit = 30.0; + } + + *rty_up = localDW->limit; + *rty_lo = -localDW->limit; + localDW->lastPsi = rtu_Psi_c; +} + +void AutopilotLawsModelClass::AutopilotLaws_Chart_Init(real_T *rty_out) +{ + *rty_out = 0.0; +} + +void AutopilotLawsModelClass::AutopilotLaws_Chart(real_T rtu_right, real_T rtu_left, boolean_T rtu_use_short_path, + real_T *rty_out, rtDW_Chart_AutopilotLaws_T *localDW) +{ + if (localDW->is_active_c10_AutopilotLaws == 0U) { + localDW->is_active_c10_AutopilotLaws = 1U; + localDW->is_c10_AutopilotLaws = AutopilotLaws_IN_any; + if (std::abs(rtu_left) < std::abs(rtu_right)) { + *rty_out = rtu_left; + } else { + *rty_out = rtu_right; + } + } else { + switch (localDW->is_c10_AutopilotLaws) { + case AutopilotLaws_IN_any: + { + real_T tmp; + real_T tmp_0; + boolean_T tmp_1; + tmp = std::abs(rtu_right); + tmp_0 = std::abs(rtu_left); + tmp_1 = !rtu_use_short_path; + if (tmp_1 && (tmp < tmp_0) && (tmp >= 10.0) && (tmp <= 20.0)) { + localDW->is_c10_AutopilotLaws = AutopilotLaws_IN_right; + *rty_out = rtu_right; + } else if (tmp_1 && (tmp_0 < tmp) && (tmp_0 >= 10.0) && (tmp_0 <= 20.0)) { + localDW->is_c10_AutopilotLaws = AutopilotLaws_IN_left; + *rty_out = rtu_left; + } else if (tmp_0 < tmp) { + *rty_out = rtu_left; + } else { + *rty_out = rtu_right; + } + } + break; + + case AutopilotLaws_IN_left: + { + real_T tmp; + real_T tmp_0; + tmp = std::abs(rtu_left); + tmp_0 = std::abs(rtu_right); + if (rtu_use_short_path || (tmp_0 < 10.0) || (tmp < 10.0)) { + localDW->is_c10_AutopilotLaws = AutopilotLaws_IN_any; + if (tmp < tmp_0) { + *rty_out = rtu_left; + } else { + *rty_out = rtu_right; + } + } else { + *rty_out = rtu_left; + } + } + break; + + default: + { + real_T tmp; + real_T tmp_0; + tmp = std::abs(rtu_left); + tmp_0 = std::abs(rtu_right); + if (rtu_use_short_path || (tmp_0 < 10.0) || (tmp < 10.0)) { + localDW->is_c10_AutopilotLaws = AutopilotLaws_IN_any; + if (tmp < tmp_0) { + *rty_out = rtu_left; + } else { + *rty_out = rtu_right; + } + } else { + *rty_out = rtu_right; + } + } + break; + } + } +} + +void AutopilotLawsModelClass::AutopilotLaws_RateLimiter_n(boolean_T rtu_u, real_T rtu_up, real_T rtu_lo, real_T rtu_Ts, + real_T rtu_init, real_T *rty_Y, rtDW_RateLimiter_AutopilotLaws_l_T *localDW) +{ + if (!localDW->pY_not_empty) { + localDW->pY = rtu_init; + localDW->pY_not_empty = true; + } + + localDW->pY += std::fmax(std::fmin(static_cast(rtu_u) - localDW->pY, std::abs(rtu_up) * rtu_Ts), -std::abs + (rtu_lo) * rtu_Ts); + *rty_Y = localDW->pY; +} + +void AutopilotLawsModelClass::AutopilotLaws_Chart_g_Init(real_T *rty_out) +{ + *rty_out = 0.0; +} + +void AutopilotLawsModelClass::AutopilotLaws_Chart_h(real_T rtu_right, real_T rtu_left, real_T rtu_use_short_path, real_T + *rty_out, rtDW_Chart_AutopilotLaws_m_T *localDW) +{ + if (localDW->is_active_c15_AutopilotLaws == 0U) { + localDW->is_active_c15_AutopilotLaws = 1U; + localDW->is_c15_AutopilotLaws = AutopilotLaws_IN_any_n; + if (std::abs(rtu_left) < std::abs(rtu_right)) { + *rty_out = rtu_left; + } else { + *rty_out = rtu_right; + } + } else { + switch (localDW->is_c15_AutopilotLaws) { + case AutopilotLaws_IN_any_n: + { + real_T tmp; + real_T tmp_0; + tmp = std::abs(rtu_right); + tmp_0 = std::abs(rtu_left); + if ((rtu_use_short_path == 0.0) && (tmp < tmp_0) && (tmp >= 10.0) && (tmp <= 20.0)) { + localDW->is_c15_AutopilotLaws = AutopilotLaws_IN_right_j; + *rty_out = rtu_right; + } else if ((rtu_use_short_path == 0.0) && (tmp_0 < tmp) && (tmp_0 >= 10.0) && (tmp_0 <= 20.0)) { + localDW->is_c15_AutopilotLaws = AutopilotLaws_IN_left_l; + *rty_out = rtu_left; + } else if (tmp_0 < tmp) { + *rty_out = rtu_left; + } else { + *rty_out = rtu_right; + } + } + break; + + case AutopilotLaws_IN_left_l: + { + real_T tmp; + real_T tmp_0; + tmp = std::abs(rtu_left); + tmp_0 = std::abs(rtu_right); + if ((rtu_use_short_path != 0.0) || (tmp_0 < 10.0) || (tmp < 10.0)) { + localDW->is_c15_AutopilotLaws = AutopilotLaws_IN_any_n; + if (tmp < tmp_0) { + *rty_out = rtu_left; + } else { + *rty_out = rtu_right; + } + } else { + *rty_out = rtu_left; + } + } + break; + + default: + { + real_T tmp; + real_T tmp_0; + tmp = std::abs(rtu_left); + tmp_0 = std::abs(rtu_right); + if ((rtu_use_short_path != 0.0) || (tmp_0 < 10.0) || (tmp < 10.0)) { + localDW->is_c15_AutopilotLaws = AutopilotLaws_IN_any_n; + if (tmp < tmp_0) { + *rty_out = rtu_left; + } else { + *rty_out = rtu_right; + } + } else { + *rty_out = rtu_right; + } + } + break; + } + } +} + +void AutopilotLawsModelClass::AutopilotLaws_storevalue(boolean_T rtu_active, real_T rtu_u, real_T *rty_y, + rtDW_storevalue_AutopilotLaws_T *localDW) +{ + if ((!rtu_active) || (!localDW->storage_not_empty)) { + localDW->storage = rtu_u; + localDW->storage_not_empty = true; + } + + *rty_y = localDW->storage; +} + +void AutopilotLawsModelClass::AutopilotLaws_LeadLagFilter(real_T rtu_U, real_T rtu_C1, real_T rtu_C2, real_T rtu_C3, + real_T rtu_C4, real_T rtu_dt, real_T *rty_Y, rtDW_LeadLagFilter_AutopilotLaws_T *localDW) +{ + real_T denom; + real_T denom_tmp; + real_T tmp; + if ((!localDW->pY_not_empty) || (!localDW->pU_not_empty)) { + localDW->pU = rtu_U; + localDW->pU_not_empty = true; + localDW->pY = rtu_U; + localDW->pY_not_empty = true; + } + + denom_tmp = rtu_dt * rtu_C4; + denom = 2.0 * rtu_C3 + denom_tmp; + tmp = rtu_dt * rtu_C2; + *rty_Y = ((2.0 * rtu_C1 + tmp) / denom * rtu_U + (tmp - 2.0 * rtu_C1) / denom * localDW->pU) + (2.0 * rtu_C3 - + denom_tmp) / denom * localDW->pY; + localDW->pY = *rty_Y; + localDW->pU = rtu_U; +} + +void AutopilotLawsModelClass::AutopilotLaws_WashoutFilter(real_T rtu_U, real_T rtu_C1, real_T rtu_dt, real_T *rty_Y, + rtDW_WashoutFilter_AutopilotLaws_T *localDW) +{ + real_T ca; + real_T denom_tmp; + if ((!localDW->pY_not_empty) || (!localDW->pU_not_empty)) { + localDW->pU = rtu_U; + localDW->pU_not_empty = true; + localDW->pY = rtu_U; + localDW->pY_not_empty = true; + } + + denom_tmp = rtu_dt * rtu_C1; + ca = 2.0 / (denom_tmp + 2.0); + *rty_Y = (2.0 - denom_tmp) / (denom_tmp + 2.0) * localDW->pY + (rtu_U * ca - localDW->pU * ca); + localDW->pY = *rty_Y; + localDW->pU = rtu_U; +} + +void AutopilotLawsModelClass::AutopilotLaws_V_LSSpeedSelection1(real_T rtu_V_c, real_T rtu_VLS, real_T *rty_y) +{ + if (rtu_V_c <= rtu_VLS) { + *rty_y = rtu_VLS - 5.0; + } else { + *rty_y = rtu_VLS; + } +} + +void AutopilotLawsModelClass::AutopilotLaws_SpeedProtectionSignalSelection(const ap_laws_output *rtu_in, real_T + rtu_VS_FD, real_T rtu_VS_AP, real_T rtu_VLS_FD, real_T rtu_VLS_AP, real_T rtu_VMAX_FD, real_T rtu_VMAX_AP, real_T + rtu_margin, real_T *rty_FD, real_T *rty_AP) +{ + real_T tmp; + if (rtu_in->input.V_c_kn <= rtu_in->data.VLS_kn) { + tmp = rtu_in->data.VLS_kn - 5.0; + } else { + tmp = rtu_in->data.VLS_kn; + } + + if (rtu_in->data.V_ias_kn < tmp + rtu_margin) { + *rty_FD = std::fmin(rtu_VS_FD, rtu_VLS_FD); + *rty_AP = std::fmin(rtu_VS_AP, rtu_VLS_AP); + } else if (rtu_in->data.V_ias_kn > rtu_in->data.VMAX_kn - rtu_margin) { + *rty_FD = std::fmax(rtu_VS_FD, rtu_VMAX_FD); + *rty_AP = std::fmax(rtu_VS_AP, rtu_VMAX_AP); + } else { + *rty_FD = rtu_VS_FD; + *rty_AP = rtu_VS_AP; + } +} + +void AutopilotLawsModelClass::AutopilotLaws_VSLimiter(real_T rtu_u, const ap_laws_output *rtu_in, real_T *rty_y) +{ + real_T limit; + limit = 9.81 / (rtu_in->data.V_tas_kn * 0.51444444444444448) * 0.15 * 57.295779513082323; + *rty_y = std::fmax(-limit, std::fmin(limit, rtu_u)); +} + +void AutopilotLawsModelClass::AutopilotLaws_VSLimiter_f(real_T rtu_u, const ap_laws_output *rtu_in, real_T *rty_y) +{ + real_T limit; + limit = 9.81 / (rtu_in->data.V_tas_kn * 0.51444444444444448) * 0.3 * 57.295779513082323; + *rty_y = std::fmax(-limit, std::fmin(limit, rtu_u)); +} + +void AutopilotLawsModelClass::AutopilotLaws_SignalEnablerGSTrack(real_T rtu_u, boolean_T rtu_e, real_T *rty_y) +{ + if (rtu_e) { + *rty_y = rtu_u; + } else { + *rty_y = 0.0; + } +} + +void AutopilotLawsModelClass::AutopilotLaws_Voter1(real_T rtu_u1, real_T rtu_u2, real_T rtu_u3, real_T *rty_Y) +{ + real_T v[3]; + int32_T tmp; + v[0] = rtu_u1; + v[1] = rtu_u2; + v[2] = rtu_u3; + if (rtu_u1 < rtu_u2) { + if (rtu_u2 < rtu_u3) { + tmp = 1; + } else if (rtu_u1 < rtu_u3) { + tmp = 2; + } else { + tmp = 0; + } + } else if (rtu_u1 < rtu_u3) { + tmp = 0; + } else if (rtu_u2 < rtu_u3) { + tmp = 2; + } else { + tmp = 1; + } + + *rty_Y = v[tmp]; +} + +void AutopilotLawsModelClass::step() +{ + static const int8_T b[5]{ 15, 30, 30, 19, 19 }; + + real_T result_tmp[9]; + real_T result[3]; + real_T result_0[3]; + real_T L; + real_T Phi2; + real_T R; + real_T a; + real_T b_L; + real_T b_R; + real_T distance_m; + real_T rtb_Add3_j4; + real_T rtb_Add3_lz; + real_T rtb_Add3_np; + real_T rtb_Cos1_j; + real_T rtb_Cos1_pk; + real_T rtb_Cos_i; + real_T rtb_FD_i; + real_T rtb_Gain1_na; + real_T rtb_Gain4; + real_T rtb_GainTheta; + real_T rtb_GainTheta1; + real_T rtb_Gain_ib; + real_T rtb_Gain_ml; + real_T rtb_Gain_pp; + real_T rtb_Max1; + real_T rtb_Product_dh; + real_T rtb_Saturation; + real_T rtb_Sum1_g; + real_T rtb_Sum2_j; + real_T rtb_Sum3_m3; + real_T rtb_Sum_es; + real_T rtb_Sum_i; + real_T rtb_Sum_kq; + real_T rtb_Vz; + real_T rtb_Y_i; + real_T rtb_Y_i0; + real_T rtb_Y_j; + real_T rtb_dme; + real_T rtb_error_c; + real_T rtb_lo_f; + real_T rtb_uDLookupTable_m; + int32_T i; + int32_T rtb_fpmtoms; + int32_T rtb_on_ground; + boolean_T guard1{ false }; + + boolean_T rtb_Compare_jy; + boolean_T rtb_Delay_j; + boolean_T rtb_valid; + boolean_T rtb_valid_m; + rtb_fpmtoms = ((AutopilotLaws_U.in.input.enabled_AP1 != 0.0) || (AutopilotLaws_U.in.input.enabled_AP2 != 0.0)); + rtb_GainTheta = AutopilotLaws_P.GainTheta_Gain * AutopilotLaws_U.in.data.Theta_deg; + rtb_GainTheta1 = AutopilotLaws_P.GainTheta1_Gain * AutopilotLaws_U.in.data.Phi_deg; + b_R = 0.017453292519943295 * rtb_GainTheta; + rtb_error_c = 0.017453292519943295 * rtb_GainTheta1; + a = std::tan(b_R); + distance_m = std::sin(rtb_error_c); + rtb_error_c = std::cos(rtb_error_c); + result_tmp[0] = 1.0; + result_tmp[3] = distance_m * a; + result_tmp[6] = rtb_error_c * a; + result_tmp[1] = 0.0; + result_tmp[4] = rtb_error_c; + result_tmp[7] = -distance_m; + result_tmp[2] = 0.0; + Phi2 = std::cos(b_R); + rtb_Sum_kq = 1.0 / Phi2; + result_tmp[5] = rtb_Sum_kq * distance_m; + result_tmp[8] = rtb_Sum_kq * rtb_error_c; + rtb_dme = AutopilotLaws_P.Gain_Gain_de * AutopilotLaws_U.in.data.p_rad_s * AutopilotLaws_P.Gainpk_Gain; + rtb_Saturation = AutopilotLaws_P.Gain_Gain_d * AutopilotLaws_U.in.data.q_rad_s * AutopilotLaws_P.Gainqk_Gain; + a = AutopilotLaws_P.Gain_Gain_m * AutopilotLaws_U.in.data.r_rad_s; + for (i = 0; i < 3; i++) { + result[i] = (result_tmp[i + 3] * rtb_Saturation + result_tmp[i] * rtb_dme) + result_tmp[i + 6] * a; + } + + rtb_dme = std::sin(b_R); + result_tmp[0] = Phi2; + result_tmp[3] = 0.0; + result_tmp[6] = -rtb_dme; + result_tmp[1] = distance_m * rtb_dme; + result_tmp[4] = rtb_error_c; + result_tmp[7] = Phi2 * distance_m; + result_tmp[2] = rtb_error_c * rtb_dme; + result_tmp[5] = 0.0 - distance_m; + result_tmp[8] = rtb_error_c * Phi2; + for (i = 0; i < 3; i++) { + result_0[i] = (result_tmp[i + 3] * AutopilotLaws_U.in.data.by_m_s2 + result_tmp[i] * AutopilotLaws_U.in.data.bx_m_s2) + + result_tmp[i + 6] * AutopilotLaws_U.in.data.bz_m_s2; + } + + if (AutopilotLaws_U.in.data.nav_dme_valid != 0.0) { + rtb_dme = AutopilotLaws_U.in.data.nav_dme_nmi; + } else if (AutopilotLaws_U.in.data.nav_loc_valid) { + a = std::sin((AutopilotLaws_U.in.data.nav_loc_position.lat - AutopilotLaws_U.in.data.aircraft_position.lat) * + 0.017453292519943295 / 2.0); + distance_m = std::sin((AutopilotLaws_U.in.data.nav_loc_position.lon - AutopilotLaws_U.in.data.aircraft_position.lon) + * 0.017453292519943295 / 2.0); + a = std::cos(0.017453292519943295 * AutopilotLaws_U.in.data.aircraft_position.lat) * std::cos(0.017453292519943295 * + AutopilotLaws_U.in.data.nav_loc_position.lat) * distance_m * distance_m + a * a; + rtb_dme = std::atan2(std::sqrt(a), std::sqrt(1.0 - a)) * 2.0 * 6.371E+6; + distance_m = AutopilotLaws_U.in.data.aircraft_position.alt - AutopilotLaws_U.in.data.nav_loc_position.alt; + rtb_dme = std::sqrt(rtb_dme * rtb_dme + distance_m * distance_m) / 1852.0; + } else { + rtb_dme = 0.0; + } + + rtb_error_c = 0.017453292519943295 * AutopilotLaws_U.in.data.aircraft_position.lat; + Phi2 = 0.017453292519943295 * AutopilotLaws_U.in.data.nav_loc_position.lat; + rtb_Saturation = 0.017453292519943295 * AutopilotLaws_U.in.data.aircraft_position.lon; + a = std::sin((AutopilotLaws_U.in.data.nav_loc_position.lat - AutopilotLaws_U.in.data.aircraft_position.lat) * + 0.017453292519943295 / 2.0); + distance_m = std::sin((AutopilotLaws_U.in.data.nav_loc_position.lon - AutopilotLaws_U.in.data.aircraft_position.lon) * + 0.017453292519943295 / 2.0); + a = std::cos(rtb_error_c) * std::cos(Phi2) * distance_m * distance_m + a * a; + distance_m = std::atan2(std::sqrt(a), std::sqrt(1.0 - a)) * 2.0 * 6.371E+6; + a = AutopilotLaws_U.in.data.aircraft_position.alt - AutopilotLaws_U.in.data.nav_loc_position.alt; + L = std::cos(Phi2); + R = 0.017453292519943295 * AutopilotLaws_U.in.data.nav_loc_position.lon - rtb_Saturation; + b_L = mod_HV94nWDg((mod_HV94nWDg(mod_HV94nWDg(360.0) + 360.0) - (mod_HV94nWDg(mod_HV94nWDg + (AutopilotLaws_U.in.data.nav_loc_magvar_deg) + 360.0) + 360.0)) + 360.0); + b_R = mod_HV94nWDg(360.0 - b_L); + if (std::abs(b_L) < std::abs(b_R)) { + b_R = -b_L; + } + + rtb_Sum_kq = std::cos(rtb_error_c); + rtb_error_c = std::sin(rtb_error_c); + L = mod_HV94nWDg(mod_HV94nWDg(mod_HV94nWDg(std::atan2(std::sin(R) * L, rtb_Sum_kq * std::sin(Phi2) - rtb_error_c * L * + std::cos(R)) * 57.295779513082323 + 360.0)) + 360.0) + 360.0; + Phi2 = mod_HV94nWDg((mod_HV94nWDg(mod_HV94nWDg(mod_HV94nWDg(mod_HV94nWDg(AutopilotLaws_U.in.data.nav_loc_deg - b_R) + + 360.0)) + 360.0) - L) + 360.0); + b_R = mod_HV94nWDg(360.0 - Phi2); + guard1 = false; + if (std::sqrt(distance_m * distance_m + a * a) / 1852.0 < 30.0) { + L = mod_HV94nWDg((mod_HV94nWDg(mod_HV94nWDg(AutopilotLaws_U.in.data.nav_loc_deg) + 360.0) - L) + 360.0); + R = mod_HV94nWDg(360.0 - L); + if (std::abs(L) < std::abs(R)) { + R = -L; + } + + if ((std::abs(R) < 90.0) && ((AutopilotLaws_U.in.data.nav_loc_position.lat != 0.0) || + (AutopilotLaws_U.in.data.nav_loc_position.lon != 0.0) || (AutopilotLaws_U.in.data.nav_loc_position.alt != 0.0))) + { + rtb_valid = true; + if (std::abs(Phi2) < std::abs(b_R)) { + b_R = -Phi2; + } + } else { + guard1 = true; + } + } else { + guard1 = true; + } + + if (guard1) { + rtb_valid = false; + b_R = 0.0; + } + + if (AutopilotLaws_U.in.data.nav_gs_valid || (!AutopilotLaws_DWork.nav_gs_deg_not_empty)) { + AutopilotLaws_DWork.nav_gs_deg = AutopilotLaws_U.in.data.nav_gs_deg; + AutopilotLaws_DWork.nav_gs_deg_not_empty = true; + } + + Phi2 = 0.017453292519943295 * AutopilotLaws_U.in.data.nav_gs_position.lat; + a = std::sin((AutopilotLaws_U.in.data.nav_gs_position.lat - AutopilotLaws_U.in.data.aircraft_position.lat) * + 0.017453292519943295 / 2.0); + distance_m = std::sin((AutopilotLaws_U.in.data.nav_gs_position.lon - AutopilotLaws_U.in.data.aircraft_position.lon) * + 0.017453292519943295 / 2.0); + L = std::cos(Phi2); + a = rtb_Sum_kq * L * distance_m * distance_m + a * a; + distance_m = std::atan2(std::sqrt(a), std::sqrt(1.0 - a)) * 2.0 * 6.371E+6; + a = AutopilotLaws_U.in.data.aircraft_position.alt - AutopilotLaws_U.in.data.nav_gs_position.alt; + distance_m = std::sqrt(distance_m * distance_m + a * a); + rtb_Saturation = 0.017453292519943295 * AutopilotLaws_U.in.data.nav_gs_position.lon - rtb_Saturation; + rtb_error_c = std::atan2(std::sin(rtb_Saturation) * L, rtb_Sum_kq * std::sin(Phi2) - rtb_error_c * L * std::cos + (rtb_Saturation)) * 57.295779513082323; + if (rtb_error_c + 360.0 == 0.0) { + rtb_Saturation = 0.0; + } else { + rtb_Saturation = std::fmod(rtb_error_c + 360.0, 360.0); + if (rtb_Saturation == 0.0) { + rtb_Saturation = 0.0; + } else if (rtb_error_c + 360.0 < 0.0) { + rtb_Saturation += 360.0; + } + } + + guard1 = false; + if (distance_m / 1852.0 < 30.0) { + if (AutopilotLaws_U.in.data.nav_loc_deg == 0.0) { + rtb_error_c = 0.0; + } else { + rtb_error_c = std::fmod(AutopilotLaws_U.in.data.nav_loc_deg, 360.0); + if (rtb_error_c == 0.0) { + rtb_error_c = 0.0; + } else if (AutopilotLaws_U.in.data.nav_loc_deg < 0.0) { + rtb_error_c += 360.0; + } + } + + if (rtb_Saturation == 0.0) { + Phi2 = 0.0; + } else { + Phi2 = std::fmod(rtb_Saturation, 360.0); + if (Phi2 == 0.0) { + Phi2 = 0.0; + } else if (rtb_Saturation < 0.0) { + Phi2 += 360.0; + } + } + + if (rtb_error_c + 360.0 == 0.0) { + rtb_Sum_kq = 0.0; + } else { + rtb_Sum_kq = std::fmod(rtb_error_c + 360.0, 360.0); + } + + if (Phi2 + 360.0 == 0.0) { + rtb_error_c = 0.0; + } else { + rtb_error_c = std::fmod(Phi2 + 360.0, 360.0); + } + + rtb_error_c = (rtb_Sum_kq - (rtb_error_c + 360.0)) + 360.0; + if (rtb_error_c == 0.0) { + L = 0.0; + } else { + L = std::fmod(rtb_error_c, 360.0); + if (L == 0.0) { + L = 0.0; + } else if (rtb_error_c < 0.0) { + L += 360.0; + } + } + + if (360.0 - L == 0.0) { + R = 0.0; + } else { + R = std::fmod(360.0 - L, 360.0); + if (R == 0.0) { + R = 0.0; + } else if (360.0 - L < 0.0) { + R += 360.0; + } + } + + if (std::abs(L) < std::abs(R)) { + R = -L; + } + + if ((std::abs(R) < 90.0) && ((AutopilotLaws_U.in.data.nav_gs_position.lat != 0.0) || + (AutopilotLaws_U.in.data.nav_gs_position.lon != 0.0) || (AutopilotLaws_U.in.data.nav_gs_position.alt != 0.0))) + { + rtb_valid_m = true; + rtb_error_c = std::asin(a / distance_m) * 57.295779513082323 - AutopilotLaws_DWork.nav_gs_deg; + } else { + guard1 = true; + } + } else { + guard1 = true; + } + + if (guard1) { + rtb_valid_m = false; + rtb_error_c = 0.0; + } + + rtb_Saturation = AutopilotLaws_P.Gain_Gain_n * AutopilotLaws_U.in.data.gear_strut_compression_1 - + AutopilotLaws_P.Constant1_Value_b; + if (rtb_Saturation > AutopilotLaws_P.Saturation_UpperSat_p) { + rtb_Saturation = AutopilotLaws_P.Saturation_UpperSat_p; + } else if (rtb_Saturation < AutopilotLaws_P.Saturation_LowerSat_g) { + rtb_Saturation = AutopilotLaws_P.Saturation_LowerSat_g; + } + + a = AutopilotLaws_P.Gain1_Gain_ll * AutopilotLaws_U.in.data.gear_strut_compression_2 - + AutopilotLaws_P.Constant1_Value_b; + if (a > AutopilotLaws_P.Saturation1_UpperSat_j) { + a = AutopilotLaws_P.Saturation1_UpperSat_j; + } else if (a < AutopilotLaws_P.Saturation1_LowerSat_d) { + a = AutopilotLaws_P.Saturation1_LowerSat_d; + } + + if (AutopilotLaws_DWork.is_active_c5_AutopilotLaws == 0U) { + AutopilotLaws_DWork.is_active_c5_AutopilotLaws = 1U; + AutopilotLaws_DWork.is_c5_AutopilotLaws = AutopilotLaws_IN_OnGround; + rtb_on_ground = 1; + } else if (AutopilotLaws_DWork.is_c5_AutopilotLaws == AutopilotLaws_IN_InAir) { + if ((rtb_Saturation > 0.05) || (a > 0.05)) { + AutopilotLaws_DWork.is_c5_AutopilotLaws = AutopilotLaws_IN_OnGround; + rtb_on_ground = 1; + } else { + rtb_on_ground = 0; + } + } else if ((rtb_Saturation == 0.0) && (a == 0.0)) { + AutopilotLaws_DWork.is_c5_AutopilotLaws = AutopilotLaws_IN_InAir; + rtb_on_ground = 0; + } else { + rtb_on_ground = 1; + } + + rtb_Compare_jy = (AutopilotLaws_U.in.data.altimeter_setting_left_mbar != AutopilotLaws_DWork.DelayInput1_DSTATE); + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_DWork.DelayInput1_DSTATE_g; + AutopilotLaws_Y.out = AutopilotLaws_P.ap_laws_output_MATLABStruct; + AutopilotLaws_Y.out.output.ap_on = rtb_fpmtoms; + AutopilotLaws_Y.out.time = AutopilotLaws_U.in.time; + AutopilotLaws_Y.out.data.aircraft_position = AutopilotLaws_U.in.data.aircraft_position; + AutopilotLaws_Y.out.data.Theta_deg = rtb_GainTheta; + AutopilotLaws_Y.out.data.Phi_deg = rtb_GainTheta1; + AutopilotLaws_Y.out.data.qk_deg_s = result[1]; + AutopilotLaws_Y.out.data.rk_deg_s = result[2]; + AutopilotLaws_Y.out.data.pk_deg_s = result[0]; + AutopilotLaws_Y.out.data.V_ias_kn = AutopilotLaws_U.in.data.V_ias_kn; + AutopilotLaws_Y.out.data.V_tas_kn = AutopilotLaws_U.in.data.V_tas_kn; + AutopilotLaws_Y.out.data.V_mach = AutopilotLaws_U.in.data.V_mach; + AutopilotLaws_Y.out.data.V_gnd_kn = AutopilotLaws_U.in.data.V_gnd_kn; + AutopilotLaws_Y.out.data.alpha_deg = AutopilotLaws_U.in.data.alpha_deg; + AutopilotLaws_Y.out.data.beta_deg = AutopilotLaws_U.in.data.beta_deg; + AutopilotLaws_Y.out.data.H_ft = AutopilotLaws_U.in.data.H_ft; + AutopilotLaws_Y.out.data.H_ind_ft = AutopilotLaws_U.in.data.H_ind_ft; + AutopilotLaws_Y.out.data.H_radio_ft = AutopilotLaws_U.in.data.H_radio_ft; + AutopilotLaws_Y.out.data.H_dot_ft_min = AutopilotLaws_U.in.data.H_dot_ft_min; + AutopilotLaws_Y.out.data.Psi_magnetic_deg = AutopilotLaws_U.in.data.Psi_magnetic_deg; + AutopilotLaws_Y.out.data.Psi_magnetic_track_deg = AutopilotLaws_U.in.data.Psi_magnetic_track_deg; + AutopilotLaws_Y.out.data.Psi_true_deg = AutopilotLaws_U.in.data.Psi_true_deg; + AutopilotLaws_Y.out.data.ax_m_s2 = result_0[0]; + AutopilotLaws_Y.out.data.ay_m_s2 = result_0[1]; + AutopilotLaws_Y.out.data.az_m_s2 = result_0[2]; + AutopilotLaws_Y.out.data.bx_m_s2 = AutopilotLaws_U.in.data.bx_m_s2; + AutopilotLaws_Y.out.data.by_m_s2 = AutopilotLaws_U.in.data.by_m_s2; + AutopilotLaws_Y.out.data.bz_m_s2 = AutopilotLaws_U.in.data.bz_m_s2; + AutopilotLaws_Y.out.data.nav_valid = AutopilotLaws_U.in.data.nav_valid; + AutopilotLaws_Y.out.data.nav_loc_deg = AutopilotLaws_U.in.data.nav_loc_deg; + AutopilotLaws_Y.out.data.nav_gs_deg = AutopilotLaws_P.Gain3_Gain_a * AutopilotLaws_U.in.data.nav_gs_deg; + AutopilotLaws_Y.out.data.nav_dme_valid = AutopilotLaws_U.in.data.nav_dme_valid; + AutopilotLaws_Y.out.data.nav_dme_nmi = rtb_dme; + AutopilotLaws_Y.out.data.nav_loc_valid = AutopilotLaws_U.in.data.nav_loc_valid; + AutopilotLaws_Y.out.data.nav_loc_magvar_deg = AutopilotLaws_U.in.data.nav_loc_magvar_deg; + AutopilotLaws_Y.out.data.nav_loc_error_deg = AutopilotLaws_U.in.data.nav_loc_error_deg; + AutopilotLaws_Y.out.data.nav_loc_position = AutopilotLaws_U.in.data.nav_loc_position; + AutopilotLaws_Y.out.data.nav_e_loc_valid = rtb_valid; + AutopilotLaws_Y.out.data.nav_e_loc_error_deg = b_R; + AutopilotLaws_Y.out.data.nav_gs_valid = AutopilotLaws_U.in.data.nav_gs_valid; + AutopilotLaws_Y.out.data.nav_gs_error_deg = AutopilotLaws_U.in.data.nav_gs_error_deg; + AutopilotLaws_Y.out.data.nav_gs_position = AutopilotLaws_U.in.data.nav_gs_position; + AutopilotLaws_Y.out.data.nav_e_gs_valid = rtb_valid_m; + AutopilotLaws_Y.out.data.nav_e_gs_error_deg = rtb_error_c; + AutopilotLaws_Y.out.data.flight_guidance_xtk_nmi = AutopilotLaws_U.in.data.flight_guidance_xtk_nmi; + AutopilotLaws_Y.out.data.flight_guidance_tae_deg = AutopilotLaws_U.in.data.flight_guidance_tae_deg; + AutopilotLaws_Y.out.data.flight_guidance_phi_deg = AutopilotLaws_U.in.data.flight_guidance_phi_deg; + AutopilotLaws_Y.out.data.flight_guidance_phi_limit_deg = AutopilotLaws_U.in.data.flight_guidance_phi_limit_deg; + AutopilotLaws_Y.out.data.flight_phase = AutopilotLaws_U.in.data.flight_phase; + AutopilotLaws_Y.out.data.V2_kn = AutopilotLaws_U.in.data.V2_kn; + AutopilotLaws_Y.out.data.VAPP_kn = AutopilotLaws_U.in.data.VAPP_kn; + AutopilotLaws_Y.out.data.VLS_kn = AutopilotLaws_U.in.data.VLS_kn; + AutopilotLaws_Y.out.data.VMAX_kn = AutopilotLaws_U.in.data.VMAX_kn; + AutopilotLaws_Y.out.data.is_flight_plan_available = AutopilotLaws_U.in.data.is_flight_plan_available; + AutopilotLaws_Y.out.data.altitude_constraint_ft = AutopilotLaws_U.in.data.altitude_constraint_ft; + AutopilotLaws_Y.out.data.thrust_reduction_altitude = AutopilotLaws_U.in.data.thrust_reduction_altitude; + AutopilotLaws_Y.out.data.thrust_reduction_altitude_go_around = + AutopilotLaws_U.in.data.thrust_reduction_altitude_go_around; + AutopilotLaws_Y.out.data.acceleration_altitude = AutopilotLaws_U.in.data.acceleration_altitude; + AutopilotLaws_Y.out.data.acceleration_altitude_engine_out = AutopilotLaws_U.in.data.acceleration_altitude_engine_out; + AutopilotLaws_Y.out.data.acceleration_altitude_go_around = AutopilotLaws_U.in.data.acceleration_altitude_go_around; + AutopilotLaws_Y.out.data.acceleration_altitude_go_around_engine_out = + AutopilotLaws_U.in.data.acceleration_altitude_go_around_engine_out; + AutopilotLaws_Y.out.data.cruise_altitude = AutopilotLaws_U.in.data.cruise_altitude; + AutopilotLaws_Y.out.data.on_ground = rtb_on_ground; + AutopilotLaws_Y.out.data.zeta_deg = AutopilotLaws_P.Gain2_Gain_b * AutopilotLaws_U.in.data.zeta_pos; + AutopilotLaws_Y.out.data.throttle_lever_1_pos = AutopilotLaws_U.in.data.throttle_lever_1_pos; + AutopilotLaws_Y.out.data.throttle_lever_2_pos = AutopilotLaws_U.in.data.throttle_lever_2_pos; + AutopilotLaws_Y.out.data.flaps_handle_index = AutopilotLaws_U.in.data.flaps_handle_index; + AutopilotLaws_Y.out.data.is_engine_operative_1 = AutopilotLaws_U.in.data.is_engine_operative_1; + AutopilotLaws_Y.out.data.is_engine_operative_2 = AutopilotLaws_U.in.data.is_engine_operative_2; + AutopilotLaws_Y.out.data.altimeter_setting_changed = (rtb_Compare_jy || + (AutopilotLaws_U.in.data.altimeter_setting_right_mbar != AutopilotLaws_DWork.DelayInput1_DSTATE)); + AutopilotLaws_Y.out.data.total_weight_kg = AutopilotLaws_U.in.data.total_weight_kg; + AutopilotLaws_Y.out.input = AutopilotLaws_U.in.input; + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_U.in.data.Psi_true_deg + AutopilotLaws_P.Constant3_Value_e; + AutopilotLaws_DWork.DelayInput1_DSTATE = (AutopilotLaws_U.in.data.Psi_magnetic_deg - + AutopilotLaws_DWork.DelayInput1_DSTATE) + AutopilotLaws_P.Constant3_Value_e; + b_R = rt_modd(AutopilotLaws_DWork.DelayInput1_DSTATE, AutopilotLaws_P.Constant3_Value_e); + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Constant3_Value_e - b_R; + rtb_error_c = rt_modd(AutopilotLaws_DWork.DelayInput1_DSTATE, AutopilotLaws_P.Constant3_Value_e); + if (b_R < rtb_error_c) { + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Gain1_Gain_h * b_R; + } else { + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Gain_Gain_e * rtb_error_c; + } + + AutopilotLaws_DWork.DelayInput1_DSTATE += AutopilotLaws_U.in.data.Psi_magnetic_track_deg; + AutopilotLaws_DWork.DelayInput1_DSTATE = rt_modd(AutopilotLaws_DWork.DelayInput1_DSTATE, + AutopilotLaws_P.Constant3_Value_b); + AutopilotLaws_DWork.DelayInput1_DSTATE += AutopilotLaws_P.Constant3_Value_b; + AutopilotLaws_DWork.DelayInput1_DSTATE = rt_modd(AutopilotLaws_DWork.DelayInput1_DSTATE, + AutopilotLaws_P.Constant3_Value_b); + rtb_error_c = AutopilotLaws_U.in.data.nav_loc_deg - AutopilotLaws_U.in.data.nav_loc_magvar_deg; + R = rt_modd(rt_modd(rtb_error_c, AutopilotLaws_P.Constant3_Value_n) + AutopilotLaws_P.Constant3_Value_n, + AutopilotLaws_P.Constant3_Value_n); + b_R = rt_modd((AutopilotLaws_DWork.DelayInput1_DSTATE - (R + AutopilotLaws_P.Constant3_Value_i)) + + AutopilotLaws_P.Constant3_Value_i, AutopilotLaws_P.Constant3_Value_i); + a = rt_modd(AutopilotLaws_P.Constant3_Value_i - b_R, AutopilotLaws_P.Constant3_Value_i); + if (AutopilotLaws_P.ManualSwitch_CurrentSetting == 1) { + rtb_Saturation = AutopilotLaws_P.Constant_Value_d; + } else { + rtb_Saturation = AutopilotLaws_U.in.input.lateral_law; + } + + rtb_valid = (rtb_Saturation == AutopilotLaws_P.CompareToConstant2_const); + if (b_R < a) { + rtb_Sum_kq = AutopilotLaws_P.Gain1_Gain * b_R; + } else { + rtb_Sum_kq = AutopilotLaws_P.Gain_Gain * a; + } + + b_R = std::abs(rtb_Sum_kq); + if (!AutopilotLaws_DWork.limit_not_empty) { + AutopilotLaws_DWork.limit = b_R; + AutopilotLaws_DWork.limit_not_empty = true; + } + + if (!rtb_valid) { + AutopilotLaws_DWork.limit = std::fmin(std::fmax(b_R, 15.0), 115.0); + } + + if (rtb_valid && (b_R < 15.0)) { + AutopilotLaws_DWork.limit = 15.0; + } + + AutopilotLaws_MATLABFunction(AutopilotLaws_P.tau_Value, AutopilotLaws_P.zeta_Value, &a, &b_L); + if (rtb_dme > AutopilotLaws_P.Saturation_UpperSat_b) { + rtb_Sum_kq = AutopilotLaws_P.Saturation_UpperSat_b; + } else if (rtb_dme < AutopilotLaws_P.Saturation_LowerSat_n) { + rtb_Sum_kq = AutopilotLaws_P.Saturation_LowerSat_n; + } else { + rtb_Sum_kq = rtb_dme; + } + + b_R = std::sin(AutopilotLaws_P.Gain1_Gain_f * AutopilotLaws_U.in.data.nav_loc_error_deg) * rtb_Sum_kq * + AutopilotLaws_P.Gain_Gain_h * b_L / AutopilotLaws_U.in.data.V_gnd_kn; + AutopilotLaws_DWork.DelayInput1_DSTATE = (AutopilotLaws_DWork.DelayInput1_DSTATE - (rt_modd(rt_modd + (AutopilotLaws_U.in.data.nav_loc_error_deg + R, AutopilotLaws_P.Constant3_Value_c2) + + AutopilotLaws_P.Constant3_Value_c2, AutopilotLaws_P.Constant3_Value_c2) + AutopilotLaws_P.Constant3_Value_p)) + + AutopilotLaws_P.Constant3_Value_p; + Phi2 = rt_modd(AutopilotLaws_DWork.DelayInput1_DSTATE, AutopilotLaws_P.Constant3_Value_p); + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Constant3_Value_p - Phi2; + distance_m = rt_modd(AutopilotLaws_DWork.DelayInput1_DSTATE, AutopilotLaws_P.Constant3_Value_p); + if (Phi2 < distance_m) { + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Gain1_Gain_p * Phi2; + } else { + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Gain_Gain_a * distance_m; + } + + if (b_R > AutopilotLaws_DWork.limit) { + b_R = AutopilotLaws_DWork.limit; + } else if (b_R < -AutopilotLaws_DWork.limit) { + b_R = -AutopilotLaws_DWork.limit; + } + + AutopilotLaws_DWork.DelayInput1_DSTATE = (AutopilotLaws_P.Gain2_Gain_i * AutopilotLaws_DWork.DelayInput1_DSTATE + b_R) + * a; + Phi2 = AutopilotLaws_DWork.DelayInput1_DSTATE * AutopilotLaws_U.in.data.V_gnd_kn; + AutopilotLaws_RateLimiter_n(rtb_Saturation == AutopilotLaws_P.CompareToConstant1_const, + AutopilotLaws_P.RateLimiterVariableTs_up, AutopilotLaws_P.RateLimiterVariableTs_lo, AutopilotLaws_U.in.time.dt, + AutopilotLaws_P.RateLimiterVariableTs_InitialCondition, &rtb_Y_i0, &AutopilotLaws_DWork.sf_RateLimiter_n); + AutopilotLaws_LagFilter(AutopilotLaws_U.in.data.nav_loc_error_deg, AutopilotLaws_P.LagFilter2_C1, + AutopilotLaws_U.in.time.dt, &a, &AutopilotLaws_DWork.sf_LagFilter_h); + b_R = AutopilotLaws_P.DiscreteDerivativeVariableTs_Gain * a; + AutopilotLaws_DWork.DelayInput1_DSTATE = b_R - AutopilotLaws_DWork.Delay_DSTATE; + AutopilotLaws_DWork.DelayInput1_DSTATE /= AutopilotLaws_U.in.time.dt; + AutopilotLaws_LagFilter(a + AutopilotLaws_P.Gain3_Gain_i * AutopilotLaws_DWork.DelayInput1_DSTATE, + AutopilotLaws_P.LagFilter_C1, AutopilotLaws_U.in.time.dt, &rtb_Y_i, &AutopilotLaws_DWork.sf_LagFilter_m); + AutopilotLaws_DWork.DelayInput1_DSTATE = rtb_error_c; + AutopilotLaws_DWork.DelayInput1_DSTATE = rt_modd(AutopilotLaws_DWork.DelayInput1_DSTATE, + AutopilotLaws_P.Constant3_Value_if); + AutopilotLaws_DWork.DelayInput1_DSTATE += AutopilotLaws_P.Constant3_Value_if; + distance_m = rt_modd(AutopilotLaws_DWork.DelayInput1_DSTATE, AutopilotLaws_P.Constant3_Value_if); + rtb_Compare_jy = (AutopilotLaws_U.in.data.H_radio_ft <= AutopilotLaws_P.CompareToConstant_const); + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_U.in.data.Psi_magnetic_deg + AutopilotLaws_P.Constant3_Value_cd; + AutopilotLaws_DWork.DelayInput1_DSTATE = (AutopilotLaws_U.in.input.Psi_c_deg - AutopilotLaws_DWork.DelayInput1_DSTATE) + + AutopilotLaws_P.Constant3_Value_cd; + rtb_error_c = rt_modd(AutopilotLaws_DWork.DelayInput1_DSTATE, AutopilotLaws_P.Constant3_Value_cd); + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Constant3_Value_cd - rtb_error_c; + AutopilotLaws_DWork.DelayInput1_DSTATE = rt_modd(AutopilotLaws_DWork.DelayInput1_DSTATE, + AutopilotLaws_P.Constant3_Value_cd); + rtb_valid = ((rtb_Saturation == AutopilotLaws_P.CompareToConstant5_const) == + AutopilotLaws_P.CompareToConstant_const_hx); + a = AutopilotLaws_P.Subsystem_Value / AutopilotLaws_U.in.time.dt; + if (!rtb_valid) { + for (i = 0; i < 100; i++) { + AutopilotLaws_DWork.Delay_DSTATE_l[i] = AutopilotLaws_P.Delay_InitialCondition; + } + } + + if (a < 1.0) { + rtb_valid_m = rtb_valid; + } else { + if (a > 100.0) { + i = 100; + } else { + i = static_cast(static_cast(std::fmod(std::trunc(a), 4.294967296E+9))); + } + + rtb_valid_m = AutopilotLaws_DWork.Delay_DSTATE_l[100U - static_cast(i)]; + } + + AutopilotLaws_Chart(rtb_error_c, AutopilotLaws_P.Gain_Gain_cy * AutopilotLaws_DWork.DelayInput1_DSTATE, rtb_valid != + rtb_valid_m, &a, &AutopilotLaws_DWork.sf_Chart); + AutopilotLaws_DWork.DelayInput1_DSTATE = look1_binlxpw(AutopilotLaws_U.in.data.V_tas_kn, + AutopilotLaws_P.ScheduledGain_BreakpointsForDimension1_h, AutopilotLaws_P.ScheduledGain_Table_o, 6U); + rtb_error_c = a * AutopilotLaws_DWork.DelayInput1_DSTATE; + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Gain_Gain_o * result[2]; + L = AutopilotLaws_P.Gain1_Gain_o * rtb_error_c + AutopilotLaws_DWork.DelayInput1_DSTATE; + AutopilotLaws_MATLABFunction_m(AutopilotLaws_U.in.input.Psi_c_deg, a, L, &rtb_error_c, &rtb_lo_f, + &AutopilotLaws_DWork.sf_MATLABFunction_m); + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_U.in.data.Psi_magnetic_track_deg + + AutopilotLaws_P.Constant3_Value_k; + AutopilotLaws_DWork.DelayInput1_DSTATE = (AutopilotLaws_U.in.input.Psi_c_deg - AutopilotLaws_DWork.DelayInput1_DSTATE) + + AutopilotLaws_P.Constant3_Value_k; + R = rt_modd(AutopilotLaws_DWork.DelayInput1_DSTATE, AutopilotLaws_P.Constant3_Value_k); + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Constant3_Value_k - R; + AutopilotLaws_DWork.DelayInput1_DSTATE = rt_modd(AutopilotLaws_DWork.DelayInput1_DSTATE, + AutopilotLaws_P.Constant3_Value_k); + rtb_valid_m = ((rtb_Saturation == AutopilotLaws_P.CompareToConstant4_const) == + AutopilotLaws_P.CompareToConstant_const_e); + a = AutopilotLaws_P.Subsystem_Value_n / AutopilotLaws_U.in.time.dt; + if (!rtb_valid_m) { + for (i = 0; i < 100; i++) { + AutopilotLaws_DWork.Delay_DSTATE_h5[i] = AutopilotLaws_P.Delay_InitialCondition_b; + } + } + + if (a < 1.0) { + rtb_Delay_j = rtb_valid_m; + } else { + if (a > 100.0) { + i = 100; + } else { + i = static_cast(static_cast(std::fmod(std::trunc(a), 4.294967296E+9))); + } + + rtb_Delay_j = AutopilotLaws_DWork.Delay_DSTATE_h5[100U - static_cast(i)]; + } + + AutopilotLaws_Chart(R, AutopilotLaws_P.Gain_Gain_p * AutopilotLaws_DWork.DelayInput1_DSTATE, rtb_valid_m != + rtb_Delay_j, &a, &AutopilotLaws_DWork.sf_Chart_ba); + AutopilotLaws_DWork.DelayInput1_DSTATE = look1_binlxpw(AutopilotLaws_U.in.data.V_tas_kn, + AutopilotLaws_P.ScheduledGain_BreakpointsForDimension1_o, AutopilotLaws_P.ScheduledGain_Table_e, 6U); + R = a * AutopilotLaws_DWork.DelayInput1_DSTATE; + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Gain_Gain_l * result[2]; + rtb_Sum_i = AutopilotLaws_P.Gain1_Gain_i4 * R + AutopilotLaws_DWork.DelayInput1_DSTATE; + AutopilotLaws_MATLABFunction_m(AutopilotLaws_U.in.input.Psi_c_deg, a, rtb_Sum_i, &rtb_Y_j, &b_L, + &AutopilotLaws_DWork.sf_MATLABFunction_e); + AutopilotLaws_MATLABFunction(AutopilotLaws_P.tau_Value_c, AutopilotLaws_P.zeta_Value_h, + &AutopilotLaws_DWork.DelayInput1_DSTATE, &R); + AutopilotLaws_RateLimiter(AutopilotLaws_U.in.data.flight_guidance_phi_deg, AutopilotLaws_P.RateLimiterVariableTs_up_h, + AutopilotLaws_P.RateLimiterVariableTs_lo_n, AutopilotLaws_U.in.time.dt, + AutopilotLaws_P.RateLimiterVariableTs_InitialCondition_l, &rtb_Gain1_na, &AutopilotLaws_DWork.sf_RateLimiter); + AutopilotLaws_LagFilter(rtb_Gain1_na, AutopilotLaws_P.LagFilter_C1_g, AutopilotLaws_U.in.time.dt, &a, + &AutopilotLaws_DWork.sf_LagFilter); + switch (static_cast(rtb_Saturation)) { + case 0: + L = rtb_GainTheta1; + break; + + case 1: + if (L > rtb_error_c) { + L = rtb_error_c; + } else if (L < rtb_lo_f) { + L = rtb_lo_f; + } + break; + + case 2: + if (rtb_Sum_i > rtb_Y_j) { + L = rtb_Y_j; + } else if (rtb_Sum_i < b_L) { + L = b_L; + } else { + L = rtb_Sum_i; + } + break; + + case 3: + rtb_Add3_lz = AutopilotLaws_P.Gain_Gain_c * AutopilotLaws_U.in.data.flight_guidance_xtk_nmi * R / + AutopilotLaws_U.in.data.V_gnd_kn; + if (rtb_Add3_lz > AutopilotLaws_P.Saturation_UpperSat) { + rtb_Add3_lz = AutopilotLaws_P.Saturation_UpperSat; + } else if (rtb_Add3_lz < AutopilotLaws_P.Saturation_LowerSat) { + rtb_Add3_lz = AutopilotLaws_P.Saturation_LowerSat; + } + + L = a - (AutopilotLaws_P.Gain2_Gain * AutopilotLaws_U.in.data.flight_guidance_tae_deg + rtb_Add3_lz) * + AutopilotLaws_DWork.DelayInput1_DSTATE * AutopilotLaws_U.in.data.V_gnd_kn; + break; + + case 4: + L = Phi2; + break; + + case 5: + rtb_error_c = rt_modd((AutopilotLaws_U.in.data.Psi_magnetic_deg - (AutopilotLaws_U.in.data.Psi_true_deg + + AutopilotLaws_P.Constant3_Value)) + AutopilotLaws_P.Constant3_Value, AutopilotLaws_P.Constant3_Value); + a = rt_modd(AutopilotLaws_P.Constant3_Value - rtb_error_c, AutopilotLaws_P.Constant3_Value); + if (rtb_error_c < a) { + rtb_Sum_kq = AutopilotLaws_P.Gain1_Gain_l * rtb_error_c; + } else { + rtb_Sum_kq = AutopilotLaws_P.Gain_Gain_g * a; + } + + rtb_error_c = rt_modd((rt_modd(rt_modd(AutopilotLaws_U.in.data.Psi_magnetic_track_deg + rtb_Sum_kq, + AutopilotLaws_P.Constant3_Value_d) + AutopilotLaws_P.Constant3_Value_d, AutopilotLaws_P.Constant3_Value_d) - + (distance_m + AutopilotLaws_P.Constant3_Value_c)) + AutopilotLaws_P.Constant3_Value_c, + AutopilotLaws_P.Constant3_Value_c); + a = rt_modd(AutopilotLaws_P.Constant3_Value_c - rtb_error_c, AutopilotLaws_P.Constant3_Value_c); + if (rtb_Y_i0 > AutopilotLaws_P.Saturation_UpperSat_a) { + rtb_Y_i0 = AutopilotLaws_P.Saturation_UpperSat_a; + } else if (rtb_Y_i0 < AutopilotLaws_P.Saturation_LowerSat_a) { + rtb_Y_i0 = AutopilotLaws_P.Saturation_LowerSat_a; + } + + if (rtb_error_c < a) { + rtb_Sum_kq = AutopilotLaws_P.Gain1_Gain_g * rtb_error_c; + } else { + rtb_Sum_kq = AutopilotLaws_P.Gain_Gain_f * a; + } + + if (rtb_Compare_jy) { + rtb_error_c = AutopilotLaws_P.k_beta_Phi_Gain * AutopilotLaws_U.in.data.beta_deg; + } else { + rtb_error_c = AutopilotLaws_P.Constant1_Value_fk; + } + + rtb_Add3_lz = (rtb_Y_i * look1_binlxpw(AutopilotLaws_U.in.data.V_tas_kn, + AutopilotLaws_P.ScheduledGain2_BreakpointsForDimension1, AutopilotLaws_P.ScheduledGain2_Table, 6U) * + AutopilotLaws_P.Gain4_Gain * look1_binlxpw(AutopilotLaws_U.in.data.H_radio_ft, + AutopilotLaws_P.ScheduledGain_BreakpointsForDimension1, AutopilotLaws_P.ScheduledGain_Table, 5U) + std::sin + (AutopilotLaws_P.Gain1_Gain_b * rtb_Sum_kq) * AutopilotLaws_U.in.data.V_gnd_kn * + AutopilotLaws_P.Gain2_Gain_g) + rtb_error_c; + if (rtb_Add3_lz > AutopilotLaws_P.Saturation1_UpperSat) { + rtb_Add3_lz = AutopilotLaws_P.Saturation1_UpperSat; + } else if (rtb_Add3_lz < AutopilotLaws_P.Saturation1_LowerSat) { + rtb_Add3_lz = AutopilotLaws_P.Saturation1_LowerSat; + } + + L = (AutopilotLaws_P.Constant_Value - rtb_Y_i0) * Phi2 + rtb_Add3_lz * rtb_Y_i0; + break; + + default: + L = AutopilotLaws_P.Constant3_Value_h; + break; + } + + rtb_error_c = std::abs(AutopilotLaws_U.in.data.V_tas_kn); + if (rtb_error_c > 600.0) { + rtb_error_c = 19.0; + } else { + int32_T low_i; + int32_T low_ip1; + i = 5; + low_i = 1; + low_ip1 = 2; + while (i > low_ip1) { + int32_T mid_i; + mid_i = (low_i + i) >> 1; + if (rtb_error_c >= (static_cast(mid_i) - 1.0) * 150.0) { + low_i = mid_i; + low_ip1 = mid_i + 1; + } else { + i = mid_i; + } + } + + rtb_error_c = (rtb_error_c - (static_cast(low_i) - 1.0) * 150.0) / static_cast(150 * low_i - (low_i + - 1) * 150); + if (rtb_error_c == 0.0) { + rtb_error_c = b[low_i - 1]; + } else if (rtb_error_c == 1.0) { + rtb_error_c = b[low_i]; + } else if (b[low_i - 1] == b[low_i]) { + rtb_error_c = b[low_i - 1]; + } else { + rtb_error_c = (1.0 - rtb_error_c) * static_cast(b[low_i - 1]) + rtb_error_c * static_cast(b[low_i]); + } + } + + if ((AutopilotLaws_U.in.input.lateral_mode != 30.0) && (AutopilotLaws_U.in.input.lateral_mode != 31.0) && + (AutopilotLaws_U.in.input.lateral_mode != 32.0) && (AutopilotLaws_U.in.input.lateral_mode != 33.0) && + (AutopilotLaws_U.in.input.lateral_mode != 34.0)) { + rtb_error_c = std::fmin(25.0, rtb_error_c); + } else if (AutopilotLaws_U.in.data.H_radio_ft < 700.0) { + rtb_error_c = 10.0; + } + + a = std::abs(AutopilotLaws_U.in.data.flight_guidance_phi_limit_deg); + if (!AutopilotLaws_DWork.pY_not_empty_j) { + AutopilotLaws_DWork.pY_e = 25.0; + AutopilotLaws_DWork.pY_not_empty_j = true; + } + + if ((AutopilotLaws_U.in.input.lateral_mode == 20.0) && (a > 0.0)) { + rtb_error_c = a; + } + + AutopilotLaws_DWork.pY_e += std::fmax(std::fmin(rtb_error_c - AutopilotLaws_DWork.pY_e, 5.0 * + AutopilotLaws_U.in.time.dt), -5.0 * AutopilotLaws_U.in.time.dt); + if (L > AutopilotLaws_DWork.pY_e) { + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_DWork.pY_e; + } else { + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Gain1_Gain_lt * AutopilotLaws_DWork.pY_e; + if (L >= AutopilotLaws_DWork.DelayInput1_DSTATE) { + AutopilotLaws_DWork.DelayInput1_DSTATE = L; + } + } + + AutopilotLaws_LagFilter(AutopilotLaws_P.Gain_Gain_lu * (AutopilotLaws_DWork.DelayInput1_DSTATE - rtb_GainTheta1), + AutopilotLaws_P.LagFilter_C1_a, AutopilotLaws_U.in.time.dt, &rtb_Y_i, &AutopilotLaws_DWork.sf_LagFilter_mp); + if (rtb_dme > AutopilotLaws_P.Saturation_UpperSat_o) { + rtb_dme = AutopilotLaws_P.Saturation_UpperSat_o; + } else if (rtb_dme < AutopilotLaws_P.Saturation_LowerSat_o) { + rtb_dme = AutopilotLaws_P.Saturation_LowerSat_o; + } + + R = std::sin(AutopilotLaws_P.Gain1_Gain_nr * AutopilotLaws_U.in.data.nav_loc_error_deg) * rtb_dme * + AutopilotLaws_P.Gain2_Gain_gs; + if (R > AutopilotLaws_P.Saturation1_UpperSat_g) { + R = AutopilotLaws_P.Saturation1_UpperSat_g; + } else if (R < AutopilotLaws_P.Saturation1_LowerSat_k) { + R = AutopilotLaws_P.Saturation1_LowerSat_k; + } + + rtb_Delay_j = (rtb_Saturation == AutopilotLaws_P.CompareToConstant_const_k); + if (!rtb_Delay_j) { + AutopilotLaws_DWork.Delay_DSTATE_h = AutopilotLaws_P.DiscreteTimeIntegratorVariableTs_InitialCondition; + } + + AutopilotLaws_DWork.Delay_DSTATE_h += AutopilotLaws_P.Gain6_Gain_b * R * + AutopilotLaws_P.DiscreteTimeIntegratorVariableTs_Gain * AutopilotLaws_U.in.time.dt; + if (AutopilotLaws_DWork.Delay_DSTATE_h > AutopilotLaws_P.DiscreteTimeIntegratorVariableTs_UpperLimit) { + AutopilotLaws_DWork.Delay_DSTATE_h = AutopilotLaws_P.DiscreteTimeIntegratorVariableTs_UpperLimit; + } else if (AutopilotLaws_DWork.Delay_DSTATE_h < AutopilotLaws_P.DiscreteTimeIntegratorVariableTs_LowerLimit) { + AutopilotLaws_DWork.Delay_DSTATE_h = AutopilotLaws_P.DiscreteTimeIntegratorVariableTs_LowerLimit; + } + + AutopilotLaws_storevalue(rtb_Delay_j, rt_modd(rt_modd(AutopilotLaws_U.in.data.nav_loc_deg - + AutopilotLaws_U.in.data.nav_loc_magvar_deg, AutopilotLaws_P.Constant3_Value_dk) + AutopilotLaws_P.Constant3_Value_dk, + AutopilotLaws_P.Constant3_Value_dk), &rtb_Y_j, &AutopilotLaws_DWork.sf_storevalue); + rtb_error_c = rt_modd((AutopilotLaws_U.in.data.Psi_true_deg - (rt_modd(rt_modd + (AutopilotLaws_U.in.data.nav_loc_error_deg + rtb_Y_j, AutopilotLaws_P.Constant3_Value_o) + + AutopilotLaws_P.Constant3_Value_o, AutopilotLaws_P.Constant3_Value_o) + AutopilotLaws_P.Constant3_Value_n1)) + + AutopilotLaws_P.Constant3_Value_n1, AutopilotLaws_P.Constant3_Value_n1); + rtb_dme = rt_modd(AutopilotLaws_P.Constant3_Value_n1 - rtb_error_c, AutopilotLaws_P.Constant3_Value_n1); + if (rtb_error_c < rtb_dme) { + rtb_Sum_kq = AutopilotLaws_P.Gain1_Gain_j * rtb_error_c; + } else { + rtb_Sum_kq = AutopilotLaws_P.Gain_Gain_i * rtb_dme; + } + + rtb_dme = rt_modd((rt_modd(rt_modd(((R * look1_binlxpw(AutopilotLaws_U.in.data.V_gnd_kn, + AutopilotLaws_P.ScheduledGain_BreakpointsForDimension1_j, AutopilotLaws_P.ScheduledGain_Table_p, 2U) + + AutopilotLaws_DWork.Delay_DSTATE_h) + AutopilotLaws_P.Gain1_Gain_fq * rtb_Sum_kq) + + AutopilotLaws_U.in.data.Psi_true_deg, AutopilotLaws_P.Constant3_Value_hr) + AutopilotLaws_P.Constant3_Value_hr, + AutopilotLaws_P.Constant3_Value_hr) - (AutopilotLaws_U.in.data.Psi_true_deg + AutopilotLaws_P.Constant3_Value_nr)) + + AutopilotLaws_P.Constant3_Value_nr, AutopilotLaws_P.Constant3_Value_nr); + AutopilotLaws_Chart_h(rtb_dme, AutopilotLaws_P.Gain_Gain_oc * rt_modd(AutopilotLaws_P.Constant3_Value_nr - rtb_dme, + AutopilotLaws_P.Constant3_Value_nr), AutopilotLaws_P.Constant1_Value_e, &rtb_error_c, + &AutopilotLaws_DWork.sf_Chart_b); + AutopilotLaws_RateLimiter_n(rtb_Delay_j, AutopilotLaws_P.RateLimiterVariableTs_up_n, + AutopilotLaws_P.RateLimiterVariableTs_lo_k, AutopilotLaws_U.in.time.dt, + AutopilotLaws_P.RateLimiterVariableTs_InitialCondition_i, &rtb_Y_j, &AutopilotLaws_DWork.sf_RateLimiter_e); + if (rtb_Y_j > AutopilotLaws_P.Saturation_UpperSat_k) { + rtb_Sum_kq = AutopilotLaws_P.Saturation_UpperSat_k; + } else if (rtb_Y_j < AutopilotLaws_P.Saturation_LowerSat_f3) { + rtb_Sum_kq = AutopilotLaws_P.Saturation_LowerSat_f3; + } else { + rtb_Sum_kq = rtb_Y_j; + } + + rtb_error_c = (AutopilotLaws_P.Gain_Gain_b * result[2] * rtb_Sum_kq + (AutopilotLaws_P.Constant_Value_a - rtb_Sum_kq) * + (AutopilotLaws_P.Gain4_Gain_o * AutopilotLaws_U.in.data.beta_deg)) + AutopilotLaws_P.Gain5_Gain_o * + rtb_error_c; + if (rtb_fpmtoms > AutopilotLaws_P.Switch_Threshold_n) { + switch (static_cast(rtb_Saturation)) { + case 0: + R = AutopilotLaws_P.beta1_Value; + break; + + case 1: + R = AutopilotLaws_P.beta1_Value_h; + break; + + case 2: + R = AutopilotLaws_P.beta1_Value_l; + break; + + case 3: + R = AutopilotLaws_P.beta1_Value_m; + break; + + case 4: + R = AutopilotLaws_P.beta1_Value_d; + break; + + case 5: + R = AutopilotLaws_P.beta1_Value_hy; + break; + + default: + R = AutopilotLaws_P.Gain3_Gain * rtb_error_c; + break; + } + } else { + R = AutopilotLaws_P.Constant1_Value; + } + + rtb_dme = rt_modd((distance_m - (AutopilotLaws_U.in.data.Psi_true_deg + AutopilotLaws_P.Constant3_Value_m)) + + AutopilotLaws_P.Constant3_Value_m, AutopilotLaws_P.Constant3_Value_m); + AutopilotLaws_Chart_h(rtb_dme, AutopilotLaws_P.Gain_Gain_fn * rt_modd(AutopilotLaws_P.Constant3_Value_m - rtb_dme, + AutopilotLaws_P.Constant3_Value_m), AutopilotLaws_P.Constant2_Value_l, &a, &AutopilotLaws_DWork.sf_Chart_h); + if (rtb_Compare_jy) { + rtb_Sum_kq = AutopilotLaws_P.Gain_Gain_ae * a + AutopilotLaws_P.Gain1_Gain_k * AutopilotLaws_U.in.data.beta_deg; + } else { + rtb_Sum_kq = AutopilotLaws_P.Constant1_Value_fk; + } + + AutopilotLaws_LagFilter(rtb_Sum_kq, AutopilotLaws_P.LagFilter1_C1, AutopilotLaws_U.in.time.dt, &rtb_Y_j, + &AutopilotLaws_DWork.sf_LagFilter_c); + switch (static_cast(rtb_Saturation)) { + case 0: + rtb_Saturation = AutopilotLaws_P.beta_Value; + break; + + case 1: + rtb_Saturation = AutopilotLaws_P.beta_Value_e; + break; + + case 2: + rtb_Saturation = AutopilotLaws_P.beta_Value_b; + break; + + case 3: + rtb_Saturation = AutopilotLaws_P.beta_Value_i; + break; + + case 4: + rtb_Saturation = AutopilotLaws_P.beta_Value_c; + break; + + case 5: + if (rtb_Y_j > AutopilotLaws_P.Saturation_UpperSat_e) { + rtb_Saturation = AutopilotLaws_P.Saturation_UpperSat_e; + } else if (rtb_Y_j < AutopilotLaws_P.Saturation_LowerSat_f) { + rtb_Saturation = AutopilotLaws_P.Saturation_LowerSat_f; + } else { + rtb_Saturation = rtb_Y_j; + } + break; + + default: + rtb_Saturation = AutopilotLaws_P.Gain7_Gain * rtb_error_c; + break; + } + + AutopilotLaws_LagFilter(rtb_Saturation, AutopilotLaws_P.LagFilter_C1_k, AutopilotLaws_U.in.time.dt, &rtb_error_c, + &AutopilotLaws_DWork.sf_LagFilter_h2); + AutopilotLaws_DWork.icLoad = ((rtb_fpmtoms == 0) || AutopilotLaws_DWork.icLoad); + if (AutopilotLaws_DWork.icLoad) { + AutopilotLaws_DWork.Delay_DSTATE_hc = rtb_GainTheta1; + } + + AutopilotLaws_DWork.DelayInput1_DSTATE -= AutopilotLaws_DWork.Delay_DSTATE_hc; + if (!AutopilotLaws_DWork.pY_not_empty) { + AutopilotLaws_DWork.pY = 5.0; + AutopilotLaws_DWork.pY_not_empty = true; + } + + if ((AutopilotLaws_U.in.input.lateral_mode == 30.0) || (AutopilotLaws_U.in.input.lateral_mode == 31.0) || + (AutopilotLaws_U.in.input.lateral_mode == 32.0) || (AutopilotLaws_U.in.input.lateral_mode == 33.0) || + (AutopilotLaws_U.in.input.lateral_mode == 34.0)) { + rtb_Sum_kq = 7.5; + } else { + rtb_Sum_kq = 5.0; + } + + AutopilotLaws_DWork.pY += std::fmax(std::fmin(rtb_Sum_kq - AutopilotLaws_DWork.pY, 2.5 * AutopilotLaws_U.in.time.dt), + -2.5 * AutopilotLaws_U.in.time.dt); + AutopilotLaws_DWork.DelayInput1_DSTATE = std::fmin(AutopilotLaws_DWork.DelayInput1_DSTATE, AutopilotLaws_DWork.pY * + AutopilotLaws_U.in.time.dt); + AutopilotLaws_DWork.Delay_DSTATE_hc += std::fmax(AutopilotLaws_DWork.DelayInput1_DSTATE, AutopilotLaws_P.Gain1_Gain_kf + * AutopilotLaws_DWork.pY * AutopilotLaws_U.in.time.dt); + AutopilotLaws_LagFilter(AutopilotLaws_DWork.Delay_DSTATE_hc, AutopilotLaws_P.LagFilter_C1_l, + AutopilotLaws_U.in.time.dt, &rtb_Y_i0, &AutopilotLaws_DWork.sf_LagFilter_o); + AutopilotLaws_RateLimiter(static_cast(rtb_fpmtoms), AutopilotLaws_P.RateLimiterVariableTs_up_b, + AutopilotLaws_P.RateLimiterVariableTs_lo_b, AutopilotLaws_U.in.time.dt, + AutopilotLaws_P.RateLimiterVariableTs_InitialCondition_il, &rtb_Y_j, &AutopilotLaws_DWork.sf_RateLimiter_d); + if (rtb_Y_j > AutopilotLaws_P.Saturation_UpperSat_m) { + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Saturation_UpperSat_m; + } else if (rtb_Y_j < AutopilotLaws_P.Saturation_LowerSat_fw) { + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Saturation_LowerSat_fw; + } else { + AutopilotLaws_DWork.DelayInput1_DSTATE = rtb_Y_j; + } + + rtb_dme = rtb_Y_i0 * AutopilotLaws_DWork.DelayInput1_DSTATE; + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Constant_Value_ii - AutopilotLaws_DWork.DelayInput1_DSTATE; + AutopilotLaws_DWork.DelayInput1_DSTATE *= rtb_GainTheta1; + AutopilotLaws_DWork.DelayInput1_DSTATE += rtb_dme; + AutopilotLaws_Y.out.output.Phi_loc_c = Phi2; + rtb_Add3_lz = AutopilotLaws_P.Gain_Gain_m3 * R; + if (rtb_Add3_lz > AutopilotLaws_P.Saturation_UpperSat_c) { + AutopilotLaws_Y.out.output.Nosewheel_c = AutopilotLaws_P.Saturation_UpperSat_c; + } else if (rtb_Add3_lz < AutopilotLaws_P.Saturation_LowerSat_d) { + AutopilotLaws_Y.out.output.Nosewheel_c = AutopilotLaws_P.Saturation_LowerSat_d; + } else { + AutopilotLaws_Y.out.output.Nosewheel_c = rtb_Add3_lz; + } + + AutopilotLaws_Y.out.output.flight_director.Beta_c_deg = rtb_error_c; + AutopilotLaws_Y.out.output.autopilot.Beta_c_deg = rtb_Saturation; + AutopilotLaws_Y.out.output.flight_director.Phi_c_deg = rtb_Y_i; + AutopilotLaws_Y.out.output.autopilot.Phi_c_deg = AutopilotLaws_DWork.DelayInput1_DSTATE; + AutopilotLaws_WashoutFilter(rtb_GainTheta, AutopilotLaws_P.WashoutFilter_C1, AutopilotLaws_U.in.time.dt, &rtb_error_c, + &AutopilotLaws_DWork.sf_WashoutFilter_f); + if (AutopilotLaws_P.ManualSwitch_CurrentSetting_b == 1) { + rtb_dme = AutopilotLaws_P.Constant_Value_m; + } else { + rtb_dme = AutopilotLaws_U.in.input.vertical_law; + } + + if (AutopilotLaws_U.in.input.ALT_soft_mode_active) { + AutopilotLaws_DWork.DelayInput1_DSTATE = (AutopilotLaws_U.in.input.V_c_kn - AutopilotLaws_U.in.data.V_ias_kn) * + AutopilotLaws_P.Gain1_Gain_bs; + if (AutopilotLaws_DWork.DelayInput1_DSTATE > AutopilotLaws_P.Saturation1_UpperSat_a) { + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Saturation1_UpperSat_a; + } else if (AutopilotLaws_DWork.DelayInput1_DSTATE < AutopilotLaws_P.Saturation1_LowerSat_i) { + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Saturation1_LowerSat_i; + } + } else { + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Constant1_Value_h; + } + + if (rtb_dme != AutopilotLaws_P.CompareToConstant5_const_e) { + AutopilotLaws_B.u = (AutopilotLaws_U.in.input.H_c_ft + AutopilotLaws_U.in.data.H_ft) - + AutopilotLaws_U.in.data.H_ind_ft; + } + + AutopilotLaws_LagFilter(AutopilotLaws_B.u - AutopilotLaws_U.in.data.H_ft, AutopilotLaws_P.LagFilter_C1_ai, + AutopilotLaws_U.in.time.dt, &rtb_Y_j, &AutopilotLaws_DWork.sf_LagFilter_g); + AutopilotLaws_DWork.DelayInput1_DSTATE += AutopilotLaws_P.Gain_Gain_ft * rtb_Y_j; + if (AutopilotLaws_DWork.DelayInput1_DSTATE > AutopilotLaws_P.Saturation_UpperSat_n) { + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Saturation_UpperSat_n; + } else if (AutopilotLaws_DWork.DelayInput1_DSTATE < AutopilotLaws_P.Saturation_LowerSat_d4) { + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Saturation_LowerSat_d4; + } + + AutopilotLaws_DWork.DelayInput1_DSTATE -= AutopilotLaws_U.in.data.H_dot_ft_min; + a = AutopilotLaws_P.ftmintoms_Gain * AutopilotLaws_DWork.DelayInput1_DSTATE; + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.kntoms_Gain * AutopilotLaws_U.in.data.V_tas_kn; + if (AutopilotLaws_DWork.DelayInput1_DSTATE > AutopilotLaws_P.Saturation_UpperSat_ar) { + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Saturation_UpperSat_ar; + } else if (AutopilotLaws_DWork.DelayInput1_DSTATE < AutopilotLaws_P.Saturation_LowerSat_n5) { + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Saturation_LowerSat_n5; + } + + rtb_Add3_lz = a / AutopilotLaws_DWork.DelayInput1_DSTATE; + if (rtb_Add3_lz > 1.0) { + rtb_Add3_lz = 1.0; + } else if (rtb_Add3_lz < -1.0) { + rtb_Add3_lz = -1.0; + } + + Phi2 = AutopilotLaws_P.Gain_Gain_k * std::asin(rtb_Add3_lz); + rtb_Compare_jy = (rtb_dme == AutopilotLaws_P.CompareToConstant1_const_c); + if (!AutopilotLaws_DWork.wasActive_not_empty_m) { + AutopilotLaws_DWork.wasActive_o = rtb_Compare_jy; + AutopilotLaws_DWork.wasActive_not_empty_m = true; + } + + distance_m = AutopilotLaws_U.in.input.H_c_ft - AutopilotLaws_U.in.data.H_ind_ft; + if (distance_m < 0.0) { + rtb_Sum_kq = -1.0; + } else { + rtb_Sum_kq = (distance_m > 0.0); + } + + a = rtb_Sum_kq * AutopilotLaws_DWork.dH_offset + distance_m; + if ((!AutopilotLaws_DWork.wasActive_o) && rtb_Compare_jy) { + AutopilotLaws_DWork.k = AutopilotLaws_U.in.data.H_dot_ft_min / a; + AutopilotLaws_DWork.dH_offset = std::abs(500.0 / std::abs(AutopilotLaws_DWork.k) - 100.0); + if (a < 0.0) { + rtb_Sum_kq = -1.0; + } else { + rtb_Sum_kq = (a > 0.0); + } + + a += rtb_Sum_kq * AutopilotLaws_DWork.dH_offset; + AutopilotLaws_DWork.k = AutopilotLaws_U.in.data.H_dot_ft_min / a; + AutopilotLaws_DWork.maxH_dot = std::abs(AutopilotLaws_U.in.data.H_dot_ft_min); + } + + a *= AutopilotLaws_DWork.k; + if (std::abs(a) > AutopilotLaws_DWork.maxH_dot) { + if (a < 0.0) { + rtb_Sum_kq = -1.0; + } else { + rtb_Sum_kq = (a > 0.0); + } + + a = rtb_Sum_kq * AutopilotLaws_DWork.maxH_dot; + } + + AutopilotLaws_DWork.wasActive_o = rtb_Compare_jy; + AutopilotLaws_DWork.DelayInput1_DSTATE = a - AutopilotLaws_U.in.data.H_dot_ft_min; + a = AutopilotLaws_P.ftmintoms_Gain_c * AutopilotLaws_DWork.DelayInput1_DSTATE; + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.kntoms_Gain_h * AutopilotLaws_U.in.data.V_tas_kn; + if (AutopilotLaws_DWork.DelayInput1_DSTATE > AutopilotLaws_P.Saturation_UpperSat_d) { + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Saturation_UpperSat_d; + } else if (AutopilotLaws_DWork.DelayInput1_DSTATE < AutopilotLaws_P.Saturation_LowerSat_nr) { + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Saturation_LowerSat_nr; + } + + rtb_Add3_lz = a / AutopilotLaws_DWork.DelayInput1_DSTATE; + if (rtb_Add3_lz > 1.0) { + rtb_Add3_lz = 1.0; + } else if (rtb_Add3_lz < -1.0) { + rtb_Add3_lz = -1.0; + } + + a = AutopilotLaws_P.Gain_Gain_es * std::asin(rtb_Add3_lz); + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.fpmtoms_Gain * AutopilotLaws_U.in.data.H_dot_ft_min; + rtb_Add3_lz = AutopilotLaws_P.kntoms_Gain_m * AutopilotLaws_U.in.data.V_gnd_kn; + if (rtb_Add3_lz > AutopilotLaws_P.Saturation_UpperSat_j) { + rtb_Add3_lz = AutopilotLaws_P.Saturation_UpperSat_j; + } else if (rtb_Add3_lz < AutopilotLaws_P.Saturation_LowerSat_i) { + rtb_Add3_lz = AutopilotLaws_P.Saturation_LowerSat_i; + } + + AutopilotLaws_DWork.DelayInput1_DSTATE = std::atan(AutopilotLaws_DWork.DelayInput1_DSTATE / rtb_Add3_lz) * + AutopilotLaws_P.Gain_Gain_e3; + R = AutopilotLaws_P.Gain1_Gain_c * rtb_GainTheta1; + AutopilotLaws_WashoutFilter(AutopilotLaws_P._Gain * (AutopilotLaws_P.GStoGS_CAS_Gain * (AutopilotLaws_P.ktstomps_Gain * + AutopilotLaws_U.in.data.V_gnd_kn)), AutopilotLaws_P.WashoutFilter_C1_e, AutopilotLaws_U.in.time.dt, &rtb_Y_j, + &AutopilotLaws_DWork.sf_WashoutFilter); + rtb_Add3_lz = AutopilotLaws_P.kntoms_Gain_b * AutopilotLaws_U.in.data.V_gnd_kn; + if (rtb_Add3_lz > AutopilotLaws_P.Saturation_UpperSat_ei) { + rtb_Add3_lz = AutopilotLaws_P.Saturation_UpperSat_ei; + } else if (rtb_Add3_lz < AutopilotLaws_P.Saturation_LowerSat_dz) { + rtb_Add3_lz = AutopilotLaws_P.Saturation_LowerSat_dz; + } + + AutopilotLaws_LeadLagFilter(rtb_Y_j - AutopilotLaws_P.g_Gain * (AutopilotLaws_P.Gain1_Gain_lp * + (AutopilotLaws_P.Gain_Gain_am * ((AutopilotLaws_P.Gain1_Gain_go * rtb_GainTheta - AutopilotLaws_P.Gain1_Gain_lx * + (AutopilotLaws_P.Gain_Gain_c1 * std::atan(AutopilotLaws_P.fpmtoms_Gain_g * AutopilotLaws_U.in.data.H_dot_ft_min / + rtb_Add3_lz))) * (AutopilotLaws_P.Constant_Value_dy - std::cos(R)) + std::sin(R) * std::sin + (AutopilotLaws_P.Gain1_Gain_pf * AutopilotLaws_U.in.data.Psi_magnetic_track_deg - AutopilotLaws_P.Gain1_Gain_e * + AutopilotLaws_U.in.data.Psi_magnetic_deg)))), AutopilotLaws_P.HighPassFilter_C1, AutopilotLaws_P.HighPassFilter_C2, + AutopilotLaws_P.HighPassFilter_C3, AutopilotLaws_P.HighPassFilter_C4, AutopilotLaws_U.in.time.dt, &rtb_Y_i0, + &AutopilotLaws_DWork.sf_LeadLagFilter); + AutopilotLaws_LeadLagFilter(AutopilotLaws_P.ktstomps_Gain_b * AutopilotLaws_U.in.data.V_ias_kn, + AutopilotLaws_P.LowPassFilter_C1, AutopilotLaws_P.LowPassFilter_C2, AutopilotLaws_P.LowPassFilter_C3, + AutopilotLaws_P.LowPassFilter_C4, AutopilotLaws_U.in.time.dt, &rtb_Y_j, &AutopilotLaws_DWork.sf_LeadLagFilter_o); + R = (rtb_Y_i0 + rtb_Y_j) * AutopilotLaws_P.ug_Gain; + rtb_Sum_kq = AutopilotLaws_P.Gain1_Gain_bf * AutopilotLaws_DWork.DelayInput1_DSTATE; + L = R + rtb_Sum_kq; + b_L = AutopilotLaws_P.Constant3_Value_nq - AutopilotLaws_P.Constant4_Value; + rtb_lo_f = (AutopilotLaws_P.Gain1_Gain_ik * R + rtb_Sum_kq) * AutopilotLaws_P.Gain_Gain_aj; + if (b_L > AutopilotLaws_P.Switch_Threshold_l) { + R = AutopilotLaws_P.Constant1_Value_g; + } else { + R = AutopilotLaws_P.Gain5_Gain * rtb_lo_f; + } + + AutopilotLaws_V_LSSpeedSelection1(AutopilotLaws_U.in.input.V_c_kn, AutopilotLaws_U.in.data.VLS_kn, &rtb_Y_j); + rtb_Gain1_na = (AutopilotLaws_U.in.data.V_ias_kn - rtb_Y_j) * AutopilotLaws_P.Gain1_Gain_oz; + if (rtb_Gain1_na <= R) { + if (b_L > AutopilotLaws_P.Switch1_Threshold) { + R = AutopilotLaws_P.Constant_Value_g; + } else { + R = AutopilotLaws_P.Gain6_Gain * rtb_lo_f; + } + + if (rtb_Gain1_na >= R) { + R = rtb_Gain1_na; + } + } + + rtb_lo_f = (AutopilotLaws_P.Gain_Gain_b0 * L - AutopilotLaws_DWork.DelayInput1_DSTATE) + R; + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.fpmtoms_Gain_a * AutopilotLaws_U.in.data.H_dot_ft_min; + rtb_Add3_lz = AutopilotLaws_P.kntoms_Gain_p * AutopilotLaws_U.in.data.V_gnd_kn; + if (rtb_Add3_lz > AutopilotLaws_P.Saturation_UpperSat_h) { + rtb_Add3_lz = AutopilotLaws_P.Saturation_UpperSat_h; + } else if (rtb_Add3_lz < AutopilotLaws_P.Saturation_LowerSat_e) { + rtb_Add3_lz = AutopilotLaws_P.Saturation_LowerSat_e; + } + + AutopilotLaws_DWork.DelayInput1_DSTATE = std::atan(AutopilotLaws_DWork.DelayInput1_DSTATE / rtb_Add3_lz) * + AutopilotLaws_P.Gain_Gain_d4; + R = AutopilotLaws_P.Gain1_Gain_j0 * rtb_GainTheta1; + AutopilotLaws_WashoutFilter(AutopilotLaws_P._Gain_h * (AutopilotLaws_P.GStoGS_CAS_Gain_m * + (AutopilotLaws_P.ktstomps_Gain_g * AutopilotLaws_U.in.data.V_gnd_kn)), AutopilotLaws_P.WashoutFilter_C1_e4, + AutopilotLaws_U.in.time.dt, &rtb_Y_j, &AutopilotLaws_DWork.sf_WashoutFilter_d); + rtb_Add3_lz = AutopilotLaws_P.kntoms_Gain_l * AutopilotLaws_U.in.data.V_gnd_kn; + if (rtb_Add3_lz > AutopilotLaws_P.Saturation_UpperSat_i) { + rtb_Add3_lz = AutopilotLaws_P.Saturation_UpperSat_i; + } else if (rtb_Add3_lz < AutopilotLaws_P.Saturation_LowerSat_h) { + rtb_Add3_lz = AutopilotLaws_P.Saturation_LowerSat_h; + } + + AutopilotLaws_LeadLagFilter(rtb_Y_j - AutopilotLaws_P.g_Gain_h * (AutopilotLaws_P.Gain1_Gain_dv * + (AutopilotLaws_P.Gain_Gain_id * ((AutopilotLaws_P.Gain1_Gain_kd * rtb_GainTheta - AutopilotLaws_P.Gain1_Gain_o4 * + (AutopilotLaws_P.Gain_Gain_bs * std::atan(AutopilotLaws_P.fpmtoms_Gain_c * AutopilotLaws_U.in.data.H_dot_ft_min / + rtb_Add3_lz))) * (AutopilotLaws_P.Constant_Value_c - std::cos(R)) + std::sin(R) * std::sin + (AutopilotLaws_P.Gain1_Gain_bk * AutopilotLaws_U.in.data.Psi_magnetic_track_deg - AutopilotLaws_P.Gain1_Gain_lxx * + AutopilotLaws_U.in.data.Psi_magnetic_deg)))), AutopilotLaws_P.HighPassFilter_C1_e, + AutopilotLaws_P.HighPassFilter_C2_c, AutopilotLaws_P.HighPassFilter_C3_f, AutopilotLaws_P.HighPassFilter_C4_c, + AutopilotLaws_U.in.time.dt, &rtb_Y_i0, &AutopilotLaws_DWork.sf_LeadLagFilter_h); + AutopilotLaws_LeadLagFilter(AutopilotLaws_P.ktstomps_Gain_i * AutopilotLaws_U.in.data.V_ias_kn, + AutopilotLaws_P.LowPassFilter_C1_n, AutopilotLaws_P.LowPassFilter_C2_a, AutopilotLaws_P.LowPassFilter_C3_o, + AutopilotLaws_P.LowPassFilter_C4_o, AutopilotLaws_U.in.time.dt, &rtb_Y_j, &AutopilotLaws_DWork.sf_LeadLagFilter_m); + R = (rtb_Y_i0 + rtb_Y_j) * AutopilotLaws_P.ug_Gain_a; + rtb_Sum_kq = AutopilotLaws_P.Gain1_Gain_hm * AutopilotLaws_DWork.DelayInput1_DSTATE; + L = R + rtb_Sum_kq; + b_L = AutopilotLaws_P.Constant1_Value_b4 - AutopilotLaws_P.Constant2_Value_c; + rtb_Gain1_na = (AutopilotLaws_P.Gain1_Gain_mz * R + rtb_Sum_kq) * AutopilotLaws_P.Gain_Gain_ie; + if (b_L > AutopilotLaws_P.Switch_Threshold_b) { + R = AutopilotLaws_P.Constant1_Value_a; + } else { + R = AutopilotLaws_P.Gain5_Gain_l * rtb_Gain1_na; + } + + rtb_Y_j = AutopilotLaws_U.in.data.V_ias_kn - AutopilotLaws_U.in.data.VMAX_kn; + rtb_Sum_i = rtb_Y_j * AutopilotLaws_P.Gain1_Gain_f1; + if (rtb_Sum_i <= R) { + if (b_L > AutopilotLaws_P.Switch1_Threshold_f) { + R = AutopilotLaws_P.Constant_Value_p; + } else { + R = AutopilotLaws_P.Gain6_Gain_j * rtb_Gain1_na; + } + + if (rtb_Sum_i >= R) { + R = rtb_Sum_i; + } + } + + R += AutopilotLaws_P.Gain_Gain_kj * L - AutopilotLaws_DWork.DelayInput1_DSTATE; + AutopilotLaws_SpeedProtectionSignalSelection(&AutopilotLaws_Y.out, a, AutopilotLaws_P.VS_Gain * a, rtb_lo_f, + AutopilotLaws_P.Gain_Gain_m0 * rtb_lo_f, R, AutopilotLaws_P.Gain_Gain_lr * R, AutopilotLaws_P.Constant_Value_ig, + &b_L, &L); + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.fpmtoms_Gain_p * AutopilotLaws_U.in.data.H_dot_ft_min; + rtb_Add3_lz = AutopilotLaws_P.kntoms_Gain_f * AutopilotLaws_U.in.data.V_gnd_kn; + if (rtb_Add3_lz > AutopilotLaws_P.Saturation_UpperSat_eik) { + rtb_Add3_lz = AutopilotLaws_P.Saturation_UpperSat_eik; + } else if (rtb_Add3_lz < AutopilotLaws_P.Saturation_LowerSat_ad) { + rtb_Add3_lz = AutopilotLaws_P.Saturation_LowerSat_ad; + } + + AutopilotLaws_DWork.DelayInput1_DSTATE = std::atan(AutopilotLaws_DWork.DelayInput1_DSTATE / rtb_Add3_lz) * + AutopilotLaws_P.Gain_Gain_e33; + R = AutopilotLaws_P.Gain1_Gain_ok * AutopilotLaws_DWork.DelayInput1_DSTATE; + a = AutopilotLaws_P.Gain1_Gain_jd * rtb_GainTheta1; + rtb_lo_f = std::cos(a); + rtb_Sum_i = std::sin(a); + a = AutopilotLaws_P.ktstomps_Gain_f * AutopilotLaws_U.in.data.V_gnd_kn; + AutopilotLaws_WashoutFilter(AutopilotLaws_P._Gain_m * (AutopilotLaws_P.GStoGS_CAS_Gain_l * a), + AutopilotLaws_P.WashoutFilter_C1_k, AutopilotLaws_U.in.time.dt, &rtb_Gain1_na, + &AutopilotLaws_DWork.sf_WashoutFilter_n); + rtb_Add3_lz = AutopilotLaws_P.kntoms_Gain_a * AutopilotLaws_U.in.data.V_gnd_kn; + if (rtb_Add3_lz > AutopilotLaws_P.Saturation_UpperSat_f) { + rtb_Add3_lz = AutopilotLaws_P.Saturation_UpperSat_f; + } else if (rtb_Add3_lz < AutopilotLaws_P.Saturation_LowerSat_c) { + rtb_Add3_lz = AutopilotLaws_P.Saturation_LowerSat_c; + } + + AutopilotLaws_LeadLagFilter(rtb_Gain1_na - AutopilotLaws_P.g_Gain_j * (AutopilotLaws_P.Gain1_Gain_ca * + (AutopilotLaws_P.Gain_Gain_ms * ((AutopilotLaws_P.Gain1_Gain_dh * rtb_GainTheta - AutopilotLaws_P.Gain1_Gain_cv * + (AutopilotLaws_P.Gain_Gain_nq * std::atan(AutopilotLaws_P.fpmtoms_Gain_h * AutopilotLaws_U.in.data.H_dot_ft_min / + rtb_Add3_lz))) * (AutopilotLaws_P.Constant_Value_l - rtb_lo_f) + rtb_Sum_i * std::sin(AutopilotLaws_P.Gain1_Gain_id * + AutopilotLaws_U.in.data.Psi_magnetic_track_deg - AutopilotLaws_P.Gain1_Gain_ct * + AutopilotLaws_U.in.data.Psi_magnetic_deg)))), AutopilotLaws_P.HighPassFilter_C1_b, + AutopilotLaws_P.HighPassFilter_C2_g, AutopilotLaws_P.HighPassFilter_C3_n, AutopilotLaws_P.HighPassFilter_C4_b, + AutopilotLaws_U.in.time.dt, &a, &AutopilotLaws_DWork.sf_LeadLagFilter_es); + AutopilotLaws_LeadLagFilter(AutopilotLaws_P.ktstomps_Gain_j * AutopilotLaws_U.in.data.V_ias_kn, + AutopilotLaws_P.LowPassFilter_C1_d, AutopilotLaws_P.LowPassFilter_C2_p, AutopilotLaws_P.LowPassFilter_C3_a, + AutopilotLaws_P.LowPassFilter_C4_b, AutopilotLaws_U.in.time.dt, &rtb_Gain1_na, + &AutopilotLaws_DWork.sf_LeadLagFilter_ja); + a = (a + rtb_Gain1_na) * AutopilotLaws_P.ug_Gain_o; + rtb_lo_f = (AutopilotLaws_P.Gain1_Gain_hu * a + R) * AutopilotLaws_P.Gain_Gain_bn; + AutopilotLaws_Voter1(AutopilotLaws_U.in.data.VLS_kn, AutopilotLaws_U.in.input.V_c_kn, AutopilotLaws_U.in.data.VMAX_kn, + &rtb_Gain1_na); + rtb_Gain1_na = (AutopilotLaws_U.in.data.V_ias_kn - rtb_Gain1_na) * AutopilotLaws_P.Gain1_Gain_hz; + rtb_Compare_jy = ((distance_m > AutopilotLaws_P.CompareToConstant6_const) && (rtb_lo_f < + AutopilotLaws_P.CompareToConstant5_const_a) && (rtb_Gain1_na < AutopilotLaws_P.CompareToConstant2_const_d) && + (rtb_dme == AutopilotLaws_P.CompareToConstant2_const_e)); + R += a; + if (rtb_Compare_jy) { + a = AutopilotLaws_P.Constant_Value_f; + } else { + if (distance_m > AutopilotLaws_P.CompareToConstant_const_l) { + a = AutopilotLaws_P.Constant1_Value_c; + } else { + a = AutopilotLaws_P.Gain5_Gain_k * rtb_lo_f; + } + + if (rtb_Gain1_na <= a) { + if (distance_m > AutopilotLaws_P.CompareToConstant4_const_o) { + a = std::fmax(AutopilotLaws_P.Constant2_Value, AutopilotLaws_P.Gain1_Gain_kg * rtb_lo_f); + } else { + a = AutopilotLaws_P.Gain6_Gain_a * rtb_lo_f; + } + + if (rtb_Gain1_na >= a) { + a = rtb_Gain1_na; + } + } + } + + rtb_Sum_i = (AutopilotLaws_P.Gain_Gain_d4y * R - AutopilotLaws_DWork.DelayInput1_DSTATE) + a; + rtb_Add3_lz = AutopilotLaws_P.kntoms_Gain_n * AutopilotLaws_U.in.data.V_tas_kn; + if (distance_m < 0.0) { + rtb_Sum_kq = -1.0; + } else { + rtb_Sum_kq = (distance_m > 0.0); + } + + if (rtb_Add3_lz > AutopilotLaws_P.Saturation_UpperSat_ju) { + rtb_Add3_lz = AutopilotLaws_P.Saturation_UpperSat_ju; + } else if (rtb_Add3_lz < AutopilotLaws_P.Saturation_LowerSat_gw) { + rtb_Add3_lz = AutopilotLaws_P.Saturation_LowerSat_gw; + } + + rtb_Add3_lz = (rtb_Sum_kq * AutopilotLaws_P.Constant3_Value_ix - AutopilotLaws_U.in.data.H_dot_ft_min) * + AutopilotLaws_P.ftmintoms_Gain_d / rtb_Add3_lz; + if (rtb_Add3_lz > 1.0) { + rtb_Add3_lz = 1.0; + } else if (rtb_Add3_lz < -1.0) { + rtb_Add3_lz = -1.0; + } + + rtb_Gain_ml = AutopilotLaws_P.Gain_Gain_nz * std::asin(rtb_Add3_lz); + rtb_Add3_lz = AutopilotLaws_P.kntoms_Gain_au * AutopilotLaws_U.in.data.V_tas_kn; + if (rtb_Add3_lz > AutopilotLaws_P.Saturation_UpperSat_l) { + rtb_Add3_lz = AutopilotLaws_P.Saturation_UpperSat_l; + } else if (rtb_Add3_lz < AutopilotLaws_P.Saturation_LowerSat_hm) { + rtb_Add3_lz = AutopilotLaws_P.Saturation_LowerSat_hm; + } + + rtb_Add3_lz = (AutopilotLaws_U.in.input.H_dot_c_fpm - AutopilotLaws_U.in.data.H_dot_ft_min) * + AutopilotLaws_P.ftmintoms_Gain_l / rtb_Add3_lz; + if (rtb_Add3_lz > 1.0) { + rtb_Add3_lz = 1.0; + } else if (rtb_Add3_lz < -1.0) { + rtb_Add3_lz = -1.0; + } + + rtb_Y_i = AutopilotLaws_P.Gain_Gain_ey * std::asin(rtb_Add3_lz); + if (!AutopilotLaws_DWork.prevVerticalLaw_not_empty) { + AutopilotLaws_DWork.prevVerticalLaw = AutopilotLaws_U.in.input.vertical_law; + AutopilotLaws_DWork.prevVerticalLaw_not_empty = true; + } + + if (!AutopilotLaws_DWork.prevTarget_not_empty) { + AutopilotLaws_DWork.prevTarget = AutopilotLaws_U.in.input.H_dot_c_fpm; + AutopilotLaws_DWork.prevTarget_not_empty = true; + } + + AutopilotLaws_DWork.islevelOffActive = (((AutopilotLaws_U.in.input.vertical_law == 4.0) && + (AutopilotLaws_DWork.prevVerticalLaw != 4.0) && (AutopilotLaws_U.in.input.H_dot_c_fpm == 0.0)) || + ((AutopilotLaws_U.in.input.H_dot_c_fpm == 0.0) && (AutopilotLaws_DWork.prevTarget > 500.0)) || + ((AutopilotLaws_U.in.input.H_dot_c_fpm == 0.0) && (AutopilotLaws_U.in.input.vertical_law == 4.0) && + AutopilotLaws_DWork.islevelOffActive)); + if (AutopilotLaws_U.in.input.vertical_mode == 50.0) { + rtb_Sum_kq = 0.3; + } else if (AutopilotLaws_DWork.islevelOffActive) { + rtb_Sum_kq = 0.1; + } else { + rtb_Sum_kq = 0.05; + } + + rtb_lo_f = 9.81 / (AutopilotLaws_U.in.data.V_tas_kn * 0.51444444444444448); + rtb_Y_i0 = rtb_lo_f * rtb_Sum_kq * 57.295779513082323; + AutopilotLaws_DWork.prevVerticalLaw = AutopilotLaws_U.in.input.vertical_law; + AutopilotLaws_DWork.prevTarget = AutopilotLaws_U.in.input.H_dot_c_fpm; + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.kntoms_Gain_o * AutopilotLaws_U.in.data.V_gnd_kn; + if (AutopilotLaws_DWork.DelayInput1_DSTATE > AutopilotLaws_P.Saturation_UpperSat_fr) { + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Saturation_UpperSat_fr; + } else if (AutopilotLaws_DWork.DelayInput1_DSTATE < AutopilotLaws_P.Saturation_LowerSat_cd) { + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Saturation_LowerSat_cd; + } + + a = std::atan(AutopilotLaws_P.fpmtoms_Gain_o * AutopilotLaws_U.in.data.H_dot_ft_min / + AutopilotLaws_DWork.DelayInput1_DSTATE) * AutopilotLaws_P.Gain_Gain_lx; + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Gain1_Gain_jn * rtb_GainTheta; + rtb_Add3_lz = AutopilotLaws_P.kntoms_Gain_d * AutopilotLaws_U.in.data.V_gnd_kn; + if (rtb_Add3_lz > AutopilotLaws_P.Saturation_UpperSat_hb) { + rtb_Add3_lz = AutopilotLaws_P.Saturation_UpperSat_hb; + } else if (rtb_Add3_lz < AutopilotLaws_P.Saturation_LowerSat_k) { + rtb_Add3_lz = AutopilotLaws_P.Saturation_LowerSat_k; + } + + R = AutopilotLaws_DWork.DelayInput1_DSTATE - std::atan(AutopilotLaws_P.fpmtoms_Gain_e * + AutopilotLaws_U.in.data.H_dot_ft_min / rtb_Add3_lz) * AutopilotLaws_P.Gain_Gain_in * AutopilotLaws_P.Gain1_Gain_ps; + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Gain1_Gain_hi * rtb_GainTheta1; + rtb_Cos_i = std::cos(AutopilotLaws_DWork.DelayInput1_DSTATE); + rtb_Cos1_pk = std::sin(AutopilotLaws_DWork.DelayInput1_DSTATE); + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Gain1_Gain_da * AutopilotLaws_U.in.data.Psi_magnetic_deg; + rtb_Sum_kq = AutopilotLaws_P.Gain1_Gain_hg * AutopilotLaws_U.in.data.Psi_magnetic_track_deg - + AutopilotLaws_DWork.DelayInput1_DSTATE; + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.ktstomps_Gain_m * AutopilotLaws_U.in.data.V_gnd_kn; + AutopilotLaws_WashoutFilter(AutopilotLaws_P._Gain_k * (AutopilotLaws_P.GStoGS_CAS_Gain_k * + AutopilotLaws_DWork.DelayInput1_DSTATE), AutopilotLaws_P.WashoutFilter_C1_o, AutopilotLaws_U.in.time.dt, + &rtb_Gain1_na, &AutopilotLaws_DWork.sf_WashoutFilter_fs); + AutopilotLaws_LeadLagFilter(rtb_Gain1_na - AutopilotLaws_P.g_Gain_m * (AutopilotLaws_P.Gain1_Gain_kdq * + (AutopilotLaws_P.Gain_Gain_b5 * (R * (AutopilotLaws_P.Constant_Value_od - rtb_Cos_i) + rtb_Cos1_pk * std::sin + (rtb_Sum_kq)))), AutopilotLaws_P.HighPassFilter_C1_g, AutopilotLaws_P.HighPassFilter_C2_l, + AutopilotLaws_P.HighPassFilter_C3_j, AutopilotLaws_P.HighPassFilter_C4_i, AutopilotLaws_U.in.time.dt, + &AutopilotLaws_DWork.DelayInput1_DSTATE, &AutopilotLaws_DWork.sf_LeadLagFilter_b); + AutopilotLaws_LeadLagFilter(AutopilotLaws_P.ktstomps_Gain_c * AutopilotLaws_U.in.data.V_ias_kn, + AutopilotLaws_P.LowPassFilter_C1_m, AutopilotLaws_P.LowPassFilter_C2_l, AutopilotLaws_P.LowPassFilter_C3_i, + AutopilotLaws_P.LowPassFilter_C4_k, AutopilotLaws_U.in.time.dt, &rtb_Gain1_na, + &AutopilotLaws_DWork.sf_LeadLagFilter_kq); + AutopilotLaws_DWork.DelayInput1_DSTATE = (AutopilotLaws_DWork.DelayInput1_DSTATE + rtb_Gain1_na) * + AutopilotLaws_P.ug_Gain_aa; + R = AutopilotLaws_P.Gain1_Gain_gf * a; + rtb_Cos_i = AutopilotLaws_DWork.DelayInput1_DSTATE + R; + rtb_Cos1_pk = AutopilotLaws_P.Constant3_Value_h1 - AutopilotLaws_P.Constant4_Value_f; + R = (AutopilotLaws_P.Gain1_Gain_ov * AutopilotLaws_DWork.DelayInput1_DSTATE + R) * AutopilotLaws_P.Gain_Gain_jy; + if (rtb_Cos1_pk > AutopilotLaws_P.Switch_Threshold_o) { + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Constant1_Value_m5; + } else { + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Gain5_Gain_h * R; + } + + AutopilotLaws_V_LSSpeedSelection1(AutopilotLaws_U.in.input.V_c_kn, AutopilotLaws_U.in.data.VLS_kn, &rtb_Gain1_na); + rtb_Gain1_na = (AutopilotLaws_U.in.data.V_ias_kn - rtb_Gain1_na) * AutopilotLaws_P.Gain1_Gain_dvi; + if (rtb_Gain1_na <= AutopilotLaws_DWork.DelayInput1_DSTATE) { + if (rtb_Cos1_pk > AutopilotLaws_P.Switch1_Threshold_c) { + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Constant_Value_b; + } else { + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Gain6_Gain_ai * R; + } + + if (rtb_Gain1_na >= AutopilotLaws_DWork.DelayInput1_DSTATE) { + AutopilotLaws_DWork.DelayInput1_DSTATE = rtb_Gain1_na; + } + } + + rtb_Sum_kq = (AutopilotLaws_P.Gain_Gain_j * rtb_Cos_i - a) + AutopilotLaws_DWork.DelayInput1_DSTATE; + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.kntoms_Gain_bq * AutopilotLaws_U.in.data.V_gnd_kn; + if (AutopilotLaws_DWork.DelayInput1_DSTATE > AutopilotLaws_P.Saturation_UpperSat_ba) { + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Saturation_UpperSat_ba; + } else if (AutopilotLaws_DWork.DelayInput1_DSTATE < AutopilotLaws_P.Saturation_LowerSat_p) { + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Saturation_LowerSat_p; + } + + a = std::atan(AutopilotLaws_P.fpmtoms_Gain_p3 * AutopilotLaws_U.in.data.H_dot_ft_min / + AutopilotLaws_DWork.DelayInput1_DSTATE) * AutopilotLaws_P.Gain_Gain_py; + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Gain1_Gain_hk * rtb_GainTheta; + rtb_Add3_lz = AutopilotLaws_P.kntoms_Gain_l5 * AutopilotLaws_U.in.data.V_gnd_kn; + if (rtb_Add3_lz > AutopilotLaws_P.Saturation_UpperSat_b3) { + rtb_Add3_lz = AutopilotLaws_P.Saturation_UpperSat_b3; + } else if (rtb_Add3_lz < AutopilotLaws_P.Saturation_LowerSat_es) { + rtb_Add3_lz = AutopilotLaws_P.Saturation_LowerSat_es; + } + + R = AutopilotLaws_DWork.DelayInput1_DSTATE - std::atan(AutopilotLaws_P.fpmtoms_Gain_j * + AutopilotLaws_U.in.data.H_dot_ft_min / rtb_Add3_lz) * AutopilotLaws_P.Gain_Gain_e5 * AutopilotLaws_P.Gain1_Gain_ja; + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Gain1_Gain_er * rtb_GainTheta1; + rtb_Cos_i = std::cos(AutopilotLaws_DWork.DelayInput1_DSTATE); + rtb_Cos1_pk = std::sin(AutopilotLaws_DWork.DelayInput1_DSTATE); + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Gain1_Gain_fl * AutopilotLaws_U.in.data.Psi_magnetic_deg; + rtb_Add3_np = AutopilotLaws_P.Gain1_Gain_ero * AutopilotLaws_U.in.data.Psi_magnetic_track_deg - + AutopilotLaws_DWork.DelayInput1_DSTATE; + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.ktstomps_Gain_a * AutopilotLaws_U.in.data.V_gnd_kn; + AutopilotLaws_WashoutFilter(AutopilotLaws_P._Gain_i * (AutopilotLaws_P.GStoGS_CAS_Gain_n * + AutopilotLaws_DWork.DelayInput1_DSTATE), AutopilotLaws_P.WashoutFilter_C1_p, AutopilotLaws_U.in.time.dt, + &rtb_Gain1_na, &AutopilotLaws_DWork.sf_WashoutFilter_j); + AutopilotLaws_LeadLagFilter(rtb_Gain1_na - AutopilotLaws_P.g_Gain_g * (AutopilotLaws_P.Gain1_Gain_hv * + (AutopilotLaws_P.Gain_Gain_mx * (R * (AutopilotLaws_P.Constant_Value_ia - rtb_Cos_i) + rtb_Cos1_pk * std::sin + (rtb_Add3_np)))), AutopilotLaws_P.HighPassFilter_C1_n, AutopilotLaws_P.HighPassFilter_C2_m, + AutopilotLaws_P.HighPassFilter_C3_k, AutopilotLaws_P.HighPassFilter_C4_h, AutopilotLaws_U.in.time.dt, + &AutopilotLaws_DWork.DelayInput1_DSTATE, &AutopilotLaws_DWork.sf_LeadLagFilter_c); + AutopilotLaws_LeadLagFilter(AutopilotLaws_P.ktstomps_Gain_o * AutopilotLaws_U.in.data.V_ias_kn, + AutopilotLaws_P.LowPassFilter_C1_l, AutopilotLaws_P.LowPassFilter_C2_c, AutopilotLaws_P.LowPassFilter_C3_g, + AutopilotLaws_P.LowPassFilter_C4_d, AutopilotLaws_U.in.time.dt, &rtb_Gain1_na, + &AutopilotLaws_DWork.sf_LeadLagFilter_p); + AutopilotLaws_DWork.DelayInput1_DSTATE = (AutopilotLaws_DWork.DelayInput1_DSTATE + rtb_Gain1_na) * + AutopilotLaws_P.ug_Gain_f; + R = AutopilotLaws_P.Gain1_Gain_ot * a; + rtb_Gain1_na = AutopilotLaws_DWork.DelayInput1_DSTATE + R; + rtb_Cos_i = AutopilotLaws_P.Constant1_Value_d - AutopilotLaws_P.Constant2_Value_k; + R = (AutopilotLaws_P.Gain1_Gain_ou * AutopilotLaws_DWork.DelayInput1_DSTATE + R) * AutopilotLaws_P.Gain_Gain_jg; + if (rtb_Cos_i > AutopilotLaws_P.Switch_Threshold_a) { + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Constant1_Value_mi; + } else { + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Gain5_Gain_g * R; + } + + rtb_Cos1_pk = rtb_Y_j * AutopilotLaws_P.Gain1_Gain_gy; + if (rtb_Cos1_pk <= AutopilotLaws_DWork.DelayInput1_DSTATE) { + if (rtb_Cos_i > AutopilotLaws_P.Switch1_Threshold_b) { + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Constant_Value_ow; + } else { + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Gain6_Gain_c * R; + } + + if (rtb_Cos1_pk >= AutopilotLaws_DWork.DelayInput1_DSTATE) { + AutopilotLaws_DWork.DelayInput1_DSTATE = rtb_Cos1_pk; + } + } + + a = (AutopilotLaws_P.Gain_Gain_dm * rtb_Gain1_na - a) + AutopilotLaws_DWork.DelayInput1_DSTATE; + AutopilotLaws_SpeedProtectionSignalSelection(&AutopilotLaws_Y.out, rtb_Y_i, std::fmax(-rtb_Y_i0, std::fmin(rtb_Y_i0, + AutopilotLaws_P.VS_Gain_h * rtb_Y_i)), rtb_Sum_kq, AutopilotLaws_P.Gain_Gain_h4 * rtb_Sum_kq, a, + AutopilotLaws_P.Gain_Gain_eq * a, AutopilotLaws_P.Constant_Value_ga, &rtb_Cos1_pk, &rtb_Cos_i); + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.kntoms_Gain_c * AutopilotLaws_U.in.data.V_gnd_kn; + if (AutopilotLaws_DWork.DelayInput1_DSTATE > AutopilotLaws_P.Saturation_UpperSat_oz) { + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Saturation_UpperSat_oz; + } else if (AutopilotLaws_DWork.DelayInput1_DSTATE < AutopilotLaws_P.Saturation_LowerSat_ou) { + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Saturation_LowerSat_ou; + } + + rtb_Add3_np = AutopilotLaws_U.in.input.FPA_c_deg - std::atan(AutopilotLaws_P.fpmtoms_Gain_ps * + AutopilotLaws_U.in.data.H_dot_ft_min / AutopilotLaws_DWork.DelayInput1_DSTATE) * AutopilotLaws_P.Gain_Gain_gt; + if (!AutopilotLaws_DWork.prevVerticalLaw_not_empty_o) { + AutopilotLaws_DWork.prevVerticalLaw_g = AutopilotLaws_U.in.input.vertical_law; + AutopilotLaws_DWork.prevVerticalLaw_not_empty_o = true; + } + + if (!AutopilotLaws_DWork.prevTarget_not_empty_e) { + AutopilotLaws_DWork.prevTarget_f = AutopilotLaws_U.in.input.FPA_c_deg; + AutopilotLaws_DWork.prevTarget_not_empty_e = true; + } + + AutopilotLaws_DWork.islevelOffActive_b = (((AutopilotLaws_U.in.input.vertical_law == 5.0) && + (AutopilotLaws_DWork.prevVerticalLaw_g != 5.0) && (AutopilotLaws_U.in.input.FPA_c_deg == 0.0)) || + ((AutopilotLaws_U.in.input.FPA_c_deg == 0.0) && (AutopilotLaws_DWork.prevTarget_f > 1.0)) || + ((AutopilotLaws_U.in.input.FPA_c_deg == 0.0) && (AutopilotLaws_U.in.input.vertical_law == 5.0) && + AutopilotLaws_DWork.islevelOffActive_b)); + if (AutopilotLaws_DWork.islevelOffActive_b) { + rtb_Sum_kq = 0.1; + } else { + rtb_Sum_kq = 0.05; + } + + rtb_Y_i0 = rtb_lo_f * rtb_Sum_kq * 57.295779513082323; + AutopilotLaws_DWork.prevVerticalLaw_g = AutopilotLaws_U.in.input.vertical_law; + AutopilotLaws_DWork.prevTarget_f = AutopilotLaws_U.in.input.FPA_c_deg; + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.kntoms_Gain_cv * AutopilotLaws_U.in.data.V_gnd_kn; + if (AutopilotLaws_DWork.DelayInput1_DSTATE > AutopilotLaws_P.Saturation_UpperSat_bb) { + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Saturation_UpperSat_bb; + } else if (AutopilotLaws_DWork.DelayInput1_DSTATE < AutopilotLaws_P.Saturation_LowerSat_a4) { + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Saturation_LowerSat_a4; + } + + a = std::atan(AutopilotLaws_P.fpmtoms_Gain_d * AutopilotLaws_U.in.data.H_dot_ft_min / + AutopilotLaws_DWork.DelayInput1_DSTATE) * AutopilotLaws_P.Gain_Gain_hv; + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Gain1_Gain_ej * rtb_GainTheta; + rtb_Add3_lz = AutopilotLaws_P.kntoms_Gain_k * AutopilotLaws_U.in.data.V_gnd_kn; + if (rtb_Add3_lz > AutopilotLaws_P.Saturation_UpperSat_pj) { + rtb_Add3_lz = AutopilotLaws_P.Saturation_UpperSat_pj; + } else if (rtb_Add3_lz < AutopilotLaws_P.Saturation_LowerSat_py) { + rtb_Add3_lz = AutopilotLaws_P.Saturation_LowerSat_py; + } + + R = AutopilotLaws_DWork.DelayInput1_DSTATE - std::atan(AutopilotLaws_P.fpmtoms_Gain_f * + AutopilotLaws_U.in.data.H_dot_ft_min / rtb_Add3_lz) * AutopilotLaws_P.Gain_Gain_bf * AutopilotLaws_P.Gain1_Gain_jv; + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Gain1_Gain_gfa * rtb_GainTheta1; + rtb_Sum_kq = std::cos(AutopilotLaws_DWork.DelayInput1_DSTATE); + rtb_Cos1_j = std::sin(AutopilotLaws_DWork.DelayInput1_DSTATE); + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Gain1_Gain_kw * AutopilotLaws_U.in.data.Psi_magnetic_deg; + rtb_Add3_j4 = AutopilotLaws_P.Gain1_Gain_j4 * AutopilotLaws_U.in.data.Psi_magnetic_track_deg - + AutopilotLaws_DWork.DelayInput1_DSTATE; + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.ktstomps_Gain_j4 * AutopilotLaws_U.in.data.V_gnd_kn; + AutopilotLaws_WashoutFilter(AutopilotLaws_P._Gain_kb * (AutopilotLaws_P.GStoGS_CAS_Gain_o * + AutopilotLaws_DWork.DelayInput1_DSTATE), AutopilotLaws_P.WashoutFilter_C1_j, AutopilotLaws_U.in.time.dt, &rtb_Y_i, + &AutopilotLaws_DWork.sf_WashoutFilter_h); + AutopilotLaws_LeadLagFilter(rtb_Y_i - AutopilotLaws_P.g_Gain_l * (AutopilotLaws_P.Gain1_Gain_n4 * + (AutopilotLaws_P.Gain_Gain_bc * (R * (AutopilotLaws_P.Constant_Value_lf - rtb_Sum_kq) + rtb_Cos1_j * std::sin + (rtb_Add3_j4)))), AutopilotLaws_P.HighPassFilter_C1_i, AutopilotLaws_P.HighPassFilter_C2_h, + AutopilotLaws_P.HighPassFilter_C3_m, AutopilotLaws_P.HighPassFilter_C4_n, AutopilotLaws_U.in.time.dt, &rtb_Gain1_na, + &AutopilotLaws_DWork.sf_LeadLagFilter_e); + AutopilotLaws_LeadLagFilter(AutopilotLaws_P.ktstomps_Gain_k * AutopilotLaws_U.in.data.V_ias_kn, + AutopilotLaws_P.LowPassFilter_C1_l4, AutopilotLaws_P.LowPassFilter_C2_po, AutopilotLaws_P.LowPassFilter_C3_f, + AutopilotLaws_P.LowPassFilter_C4_dt, AutopilotLaws_U.in.time.dt, &rtb_Y_i, &AutopilotLaws_DWork.sf_LeadLagFilter_kp); + AutopilotLaws_DWork.DelayInput1_DSTATE = (rtb_Gain1_na + rtb_Y_i) * AutopilotLaws_P.ug_Gain_n; + R = AutopilotLaws_P.Gain1_Gain_b1 * a; + rtb_Gain1_na = AutopilotLaws_DWork.DelayInput1_DSTATE + R; + rtb_Sum_kq = AutopilotLaws_P.Constant3_Value_nk - AutopilotLaws_P.Constant4_Value_o; + R = (AutopilotLaws_P.Gain1_Gain_on * AutopilotLaws_DWork.DelayInput1_DSTATE + R) * AutopilotLaws_P.Gain_Gain_hy; + if (rtb_Sum_kq > AutopilotLaws_P.Switch_Threshold_d) { + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Constant1_Value_m; + } else { + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Gain5_Gain_b * R; + } + + AutopilotLaws_V_LSSpeedSelection1(AutopilotLaws_U.in.input.V_c_kn, AutopilotLaws_U.in.data.VLS_kn, &rtb_Y_i); + rtb_Y_i = (AutopilotLaws_U.in.data.V_ias_kn - rtb_Y_i) * AutopilotLaws_P.Gain1_Gain_m1; + if (rtb_Y_i <= AutopilotLaws_DWork.DelayInput1_DSTATE) { + if (rtb_Sum_kq > AutopilotLaws_P.Switch1_Threshold_d) { + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Constant_Value_p0; + } else { + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Gain6_Gain_n * R; + } + + if (rtb_Y_i >= AutopilotLaws_DWork.DelayInput1_DSTATE) { + AutopilotLaws_DWork.DelayInput1_DSTATE = rtb_Y_i; + } + } + + rtb_Cos1_j = (AutopilotLaws_P.Gain_Gain_d0 * rtb_Gain1_na - a) + AutopilotLaws_DWork.DelayInput1_DSTATE; + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.kntoms_Gain_hi * AutopilotLaws_U.in.data.V_gnd_kn; + if (AutopilotLaws_DWork.DelayInput1_DSTATE > AutopilotLaws_P.Saturation_UpperSat_cv) { + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Saturation_UpperSat_cv; + } else if (AutopilotLaws_DWork.DelayInput1_DSTATE < AutopilotLaws_P.Saturation_LowerSat_hd) { + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Saturation_LowerSat_hd; + } + + a = std::atan(AutopilotLaws_P.fpmtoms_Gain_o2 * AutopilotLaws_U.in.data.H_dot_ft_min / + AutopilotLaws_DWork.DelayInput1_DSTATE) * AutopilotLaws_P.Gain_Gain_pp; + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Gain1_Gain_iw * rtb_GainTheta; + rtb_Sum_kq = AutopilotLaws_P.kntoms_Gain_i * AutopilotLaws_U.in.data.V_gnd_kn; + if (rtb_Sum_kq > AutopilotLaws_P.Saturation_UpperSat_nu) { + rtb_Sum_kq = AutopilotLaws_P.Saturation_UpperSat_nu; + } else if (rtb_Sum_kq < AutopilotLaws_P.Saturation_LowerSat_ae) { + rtb_Sum_kq = AutopilotLaws_P.Saturation_LowerSat_ae; + } + + R = AutopilotLaws_DWork.DelayInput1_DSTATE - std::atan(AutopilotLaws_P.fpmtoms_Gain_hz * + AutopilotLaws_U.in.data.H_dot_ft_min / rtb_Sum_kq) * AutopilotLaws_P.Gain_Gain_ej * AutopilotLaws_P.Gain1_Gain_lw; + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Gain1_Gain_ky * rtb_GainTheta1; + rtb_Sum_kq = std::cos(AutopilotLaws_DWork.DelayInput1_DSTATE); + rtb_Add3_j4 = std::sin(AutopilotLaws_DWork.DelayInput1_DSTATE); + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Gain1_Gain_nrn * AutopilotLaws_U.in.data.Psi_magnetic_deg; + rtb_Add3_lz = AutopilotLaws_P.Gain1_Gain_ip * AutopilotLaws_U.in.data.Psi_magnetic_track_deg - + AutopilotLaws_DWork.DelayInput1_DSTATE; + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.ktstomps_Gain_l * AutopilotLaws_U.in.data.V_gnd_kn; + AutopilotLaws_WashoutFilter(AutopilotLaws_P._Gain_ip * (AutopilotLaws_P.GStoGS_CAS_Gain_e * + AutopilotLaws_DWork.DelayInput1_DSTATE), AutopilotLaws_P.WashoutFilter_C1_c, AutopilotLaws_U.in.time.dt, &rtb_Y_i, + &AutopilotLaws_DWork.sf_WashoutFilter_g5); + AutopilotLaws_LeadLagFilter(rtb_Y_i - AutopilotLaws_P.g_Gain_hq * (AutopilotLaws_P.Gain1_Gain_mx * + (AutopilotLaws_P.Gain_Gain_d3 * (R * (AutopilotLaws_P.Constant_Value_fo - rtb_Sum_kq) + rtb_Add3_j4 * std::sin + (rtb_Add3_lz)))), AutopilotLaws_P.HighPassFilter_C1_d, AutopilotLaws_P.HighPassFilter_C2_i, + AutopilotLaws_P.HighPassFilter_C3_d, AutopilotLaws_P.HighPassFilter_C4_nr, AutopilotLaws_U.in.time.dt, &rtb_Gain1_na, + &AutopilotLaws_DWork.sf_LeadLagFilter_j); + AutopilotLaws_LeadLagFilter(AutopilotLaws_P.ktstomps_Gain_mh * AutopilotLaws_U.in.data.V_ias_kn, + AutopilotLaws_P.LowPassFilter_C1_e, AutopilotLaws_P.LowPassFilter_C2_i, AutopilotLaws_P.LowPassFilter_C3_o5, + AutopilotLaws_P.LowPassFilter_C4_f, AutopilotLaws_U.in.time.dt, &rtb_Y_i, &AutopilotLaws_DWork.sf_LeadLagFilter_a); + AutopilotLaws_DWork.DelayInput1_DSTATE = (rtb_Gain1_na + rtb_Y_i) * AutopilotLaws_P.ug_Gain_e; + R = AutopilotLaws_P.Gain1_Gain_be * a; + rtb_Gain1_na = AutopilotLaws_DWork.DelayInput1_DSTATE + R; + rtb_Y_i = AutopilotLaws_P.Constant1_Value_o - AutopilotLaws_P.Constant2_Value_h; + R = (AutopilotLaws_P.Gain1_Gain_nj * AutopilotLaws_DWork.DelayInput1_DSTATE + R) * AutopilotLaws_P.Gain_Gain_aq; + if (rtb_Y_i > AutopilotLaws_P.Switch_Threshold_g) { + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Constant1_Value_f; + } else { + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Gain5_Gain_a * R; + } + + rtb_Y_j *= AutopilotLaws_P.Gain1_Gain_fle; + if (rtb_Y_j <= AutopilotLaws_DWork.DelayInput1_DSTATE) { + if (rtb_Y_i > AutopilotLaws_P.Switch1_Threshold_h) { + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Constant_Value_i; + } else { + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Gain6_Gain_g * R; + } + + if (rtb_Y_j >= AutopilotLaws_DWork.DelayInput1_DSTATE) { + AutopilotLaws_DWork.DelayInput1_DSTATE = rtb_Y_j; + } + } + + a = (AutopilotLaws_P.Gain_Gain_gx * rtb_Gain1_na - a) + AutopilotLaws_DWork.DelayInput1_DSTATE; + AutopilotLaws_SpeedProtectionSignalSelection(&AutopilotLaws_Y.out, rtb_Add3_np, std::fmax(-rtb_Y_i0, std::fmin + (rtb_Y_i0, AutopilotLaws_P.Gain_Gain_c3 * rtb_Add3_np)), rtb_Cos1_j, AutopilotLaws_P.Gain_Gain_fnw * rtb_Cos1_j, a, + AutopilotLaws_P.Gain_Gain_ko * a, AutopilotLaws_P.Constant_Value_fov, &rtb_FD_i, &rtb_Add3_j4); + rtb_Add3_np = AutopilotLaws_P.Gain2_Gain_n * AutopilotLaws_U.in.data.H_dot_ft_min * + AutopilotLaws_P.DiscreteDerivativeVariableTs1_Gain; + AutopilotLaws_LagFilter((rtb_Add3_np - AutopilotLaws_DWork.Delay_DSTATE_c) / AutopilotLaws_U.in.time.dt, + AutopilotLaws_P.LagFilter2_C1_d, AutopilotLaws_U.in.time.dt, &AutopilotLaws_DWork.DelayInput1_DSTATE, + &AutopilotLaws_DWork.sf_LagFilter_f); + a = AutopilotLaws_P.kn2ms_Gain * AutopilotLaws_U.in.data.V_gnd_kn; + AutopilotLaws_LagFilter(AutopilotLaws_P.Gain_Gain_os * (std::tan(AutopilotLaws_P.Gain1_Gain_ox * result[1]) * a), + AutopilotLaws_P.LagFilter3_C1, AutopilotLaws_U.in.time.dt, &rtb_Gain1_na, &AutopilotLaws_DWork.sf_LagFilter_l); + AutopilotLaws_LagFilter(AutopilotLaws_DWork.DelayInput1_DSTATE - rtb_Gain1_na, AutopilotLaws_P.LagFilter4_C1, + AutopilotLaws_U.in.time.dt, &a, &AutopilotLaws_DWork.sf_LagFilter_i); + AutopilotLaws_WashoutFilter(a, AutopilotLaws_P.WashoutFilter1_C1, AutopilotLaws_U.in.time.dt, &rtb_Gain1_na, + &AutopilotLaws_DWork.sf_WashoutFilter_db); + rtb_Add3_lz = AutopilotLaws_P.Gain4_Gain_n * rtb_Gain1_na; + AutopilotLaws_LagFilter(AutopilotLaws_U.in.data.nav_gs_error_deg, AutopilotLaws_P.LagFilter1_C1_l, + AutopilotLaws_U.in.time.dt, &rtb_Gain1_na, &AutopilotLaws_DWork.sf_LagFilter_gx); + R = rtb_Gain1_na * look1_binlxpw(AutopilotLaws_U.in.data.H_radio_ft, + AutopilotLaws_P.ScheduledGain_BreakpointsForDimension1_a, AutopilotLaws_P.ScheduledGain_Table_j, 7U) * + AutopilotLaws_P.Gain_Gain_gm; + rtb_Delay_j = ((AutopilotLaws_U.in.input.vertical_mode == AutopilotLaws_P.CompareGSTRACK_const) || + (AutopilotLaws_U.in.input.vertical_mode == AutopilotLaws_P.CompareGSTRACK2_const)); + AutopilotLaws_RateLimiter_n(rtb_Delay_j, AutopilotLaws_P.RateLimiterVariableTs_up_d, + AutopilotLaws_P.RateLimiterVariableTs_lo_c, AutopilotLaws_U.in.time.dt, + AutopilotLaws_P.RateLimiterVariableTs_InitialCondition_m, &rtb_Gain1_na, &AutopilotLaws_DWork.sf_RateLimiter_l); + if (rtb_Gain1_na > AutopilotLaws_P.Saturation_UpperSat_j1) { + a = AutopilotLaws_P.Saturation_UpperSat_j1; + } else if (rtb_Gain1_na < AutopilotLaws_P.Saturation_LowerSat_nq) { + a = AutopilotLaws_P.Saturation_LowerSat_nq; + } else { + a = rtb_Gain1_na; + } + + AutopilotLaws_LagFilter(AutopilotLaws_U.in.data.nav_gs_error_deg, AutopilotLaws_P.LagFilter2_C1_e, + AutopilotLaws_U.in.time.dt, &rtb_Y_i, &AutopilotLaws_DWork.sf_LagFilter_cf); + rtb_Cos1_j = AutopilotLaws_P.DiscreteDerivativeVariableTs_Gain_g * rtb_Y_i; + AutopilotLaws_DWork.DelayInput1_DSTATE = look1_binlxpw(AutopilotLaws_U.in.data.H_radio_ft, + AutopilotLaws_P.ScheduledGain3_BreakpointsForDimension1, AutopilotLaws_P.ScheduledGain3_Table, 4U); + AutopilotLaws_LagFilter(rtb_Y_i + (rtb_Cos1_j - AutopilotLaws_DWork.Delay_DSTATE_b) / AutopilotLaws_U.in.time.dt * + AutopilotLaws_DWork.DelayInput1_DSTATE, AutopilotLaws_P.LagFilter_C1_d, AutopilotLaws_U.in.time.dt, &rtb_Gain1_na, + &AutopilotLaws_DWork.sf_LagFilter_p); + if (rtb_Add3_lz > AutopilotLaws_P.Saturation_UpperSat_ko) { + rtb_Add3_lz = AutopilotLaws_P.Saturation_UpperSat_ko; + } else if (rtb_Add3_lz < AutopilotLaws_P.Saturation_LowerSat_ez) { + rtb_Add3_lz = AutopilotLaws_P.Saturation_LowerSat_ez; + } + + AutopilotLaws_SignalEnablerGSTrack(AutopilotLaws_P.Gain3_Gain_c * ((AutopilotLaws_P.Gain2_Gain_k * rtb_Add3_lz + R) * + a + (AutopilotLaws_P.Constant_Value_lu - a) * (rtb_Gain1_na * look1_binlxpw(AutopilotLaws_U.in.data.H_radio_ft, + AutopilotLaws_P.ScheduledGain2_BreakpointsForDimension1_h, AutopilotLaws_P.ScheduledGain2_Table_p, 7U))), + (AutopilotLaws_U.in.data.H_radio_ft > AutopilotLaws_P.CompareToConstant_const_kt) && + AutopilotLaws_U.in.data.nav_gs_valid, &rtb_Sum_kq); + AutopilotLaws_storevalue(rtb_dme == AutopilotLaws_P.CompareToConstant6_const_e, AutopilotLaws_Y.out.data.nav_gs_deg, + &rtb_Gain1_na, &AutopilotLaws_DWork.sf_storevalue_g); + if (rtb_Gain1_na > AutopilotLaws_P.Saturation_UpperSat_e0) { + a = AutopilotLaws_P.Saturation_UpperSat_e0; + } else if (rtb_Gain1_na < AutopilotLaws_P.Saturation_LowerSat_ph) { + a = AutopilotLaws_P.Saturation_LowerSat_ph; + } else { + a = rtb_Gain1_na; + } + + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.fpmtoms_Gain_g4 * AutopilotLaws_U.in.data.H_dot_ft_min; + rtb_Add3_lz = AutopilotLaws_P.kntoms_Gain_k4 * AutopilotLaws_U.in.data.V_gnd_kn; + if (rtb_Add3_lz > AutopilotLaws_P.Saturation_UpperSat_eb) { + rtb_Add3_lz = AutopilotLaws_P.Saturation_UpperSat_eb; + } else if (rtb_Add3_lz < AutopilotLaws_P.Saturation_LowerSat_gk) { + rtb_Add3_lz = AutopilotLaws_P.Saturation_LowerSat_gk; + } + + AutopilotLaws_DWork.DelayInput1_DSTATE = std::atan(AutopilotLaws_DWork.DelayInput1_DSTATE / rtb_Add3_lz) * + AutopilotLaws_P.Gain_Gain_ow; + AutopilotLaws_SignalEnablerGSTrack(AutopilotLaws_P.Gain2_Gain_l * (a - AutopilotLaws_DWork.DelayInput1_DSTATE), + rtb_Delay_j, &rtb_Gain1_na); + AutopilotLaws_Voter1(rtb_Sum_kq + rtb_Gain1_na, AutopilotLaws_P.Gain1_Gain_d4 * ((a + AutopilotLaws_P.Bias_Bias) - + AutopilotLaws_DWork.DelayInput1_DSTATE), AutopilotLaws_P.Gain_Gain_eyl * ((a + AutopilotLaws_P.Bias1_Bias) - + AutopilotLaws_DWork.DelayInput1_DSTATE), &R); + rtb_Product_dh = R * look1_binlxpw(AutopilotLaws_U.in.data.V_tas_kn, + AutopilotLaws_P.ScheduledGain1_BreakpointsForDimension1, AutopilotLaws_P.ScheduledGain1_Table, 6U); + rtb_Gain4 = (rtb_GainTheta - AutopilotLaws_P.Constant2_Value_f) * AutopilotLaws_P.Gain4_Gain_oy; + rtb_Y_i = AutopilotLaws_P.Gain5_Gain_c * AutopilotLaws_U.in.data.bz_m_s2; + AutopilotLaws_WashoutFilter(AutopilotLaws_U.in.data.bx_m_s2, AutopilotLaws_P.WashoutFilter_C1_m, + AutopilotLaws_U.in.time.dt, &rtb_Y_i0, &AutopilotLaws_DWork.sf_WashoutFilter_g); + rtb_Delay_j = (rtb_dme == AutopilotLaws_P.CompareToConstant7_const); + AutopilotLaws_LagFilter(AutopilotLaws_U.in.data.H_dot_ft_min, AutopilotLaws_P.LagFilterH_C1, + AutopilotLaws_U.in.time.dt, &rtb_Y_j, &AutopilotLaws_DWork.sf_LagFilter_a); + a = rtb_Y_j - AutopilotLaws_P.kntofpm_Gain * AutopilotLaws_U.in.data.V_gnd_kn * AutopilotLaws_P.maxslope_Gain; + AutopilotLaws_LeadLagFilter(AutopilotLaws_U.in.data.H_radio_ft, AutopilotLaws_P.LeadLagFilter_C1, + AutopilotLaws_P.LeadLagFilter_C2, AutopilotLaws_P.LeadLagFilter_C3, AutopilotLaws_P.LeadLagFilter_C4, + AutopilotLaws_U.in.time.dt, &rtb_Y_j, &AutopilotLaws_DWork.sf_LeadLagFilter_k); + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Gain1_Gain_oa * rtb_Y_j; + rtb_Max1 = std::fmax(a, AutopilotLaws_DWork.DelayInput1_DSTATE); + if (!AutopilotLaws_DWork.wasActive_not_empty) { + AutopilotLaws_DWork.wasActive = rtb_Delay_j; + AutopilotLaws_DWork.wasActive_not_empty = true; + } + + if ((!AutopilotLaws_DWork.wasActive) && rtb_Delay_j) { + R = std::abs(rtb_Max1) / 60.0; + AutopilotLaws_DWork.Tau = AutopilotLaws_U.in.data.H_radio_ft / (R - 2.5); + AutopilotLaws_DWork.H_bias = AutopilotLaws_DWork.Tau * R - AutopilotLaws_U.in.data.H_radio_ft; + } + + if (rtb_Delay_j) { + rtb_Vz = -1.0 / AutopilotLaws_DWork.Tau * (AutopilotLaws_U.in.data.H_radio_ft + AutopilotLaws_DWork.H_bias) * 60.0; + } else { + rtb_Vz = rtb_Max1; + } + + AutopilotLaws_DWork.wasActive = rtb_Delay_j; + AutopilotLaws_LeadLagFilter(rtb_Vz, AutopilotLaws_P.LeadLagFilter_C1_a, AutopilotLaws_P.LeadLagFilter_C2_p, + AutopilotLaws_P.LeadLagFilter_C3_m, AutopilotLaws_P.LeadLagFilter_C4_k, AutopilotLaws_U.in.time.dt, &rtb_Gain1_na, + &AutopilotLaws_DWork.sf_LeadLagFilter_hp); + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.kntoms_Gain_av * AutopilotLaws_U.in.data.V_gnd_kn; + if (AutopilotLaws_DWork.DelayInput1_DSTATE > AutopilotLaws_P.Saturation_UpperSat_i0) { + rtb_Sum_kq = AutopilotLaws_P.Saturation_UpperSat_i0; + } else if (AutopilotLaws_DWork.DelayInput1_DSTATE < AutopilotLaws_P.Saturation_LowerSat_nd) { + rtb_Sum_kq = AutopilotLaws_P.Saturation_LowerSat_nd; + } else { + rtb_Sum_kq = AutopilotLaws_DWork.DelayInput1_DSTATE; + } + + R = AutopilotLaws_P.ftmintoms_Gain_k * rtb_Gain1_na / rtb_Sum_kq; + if (AutopilotLaws_DWork.DelayInput1_DSTATE > AutopilotLaws_P.Saturation_UpperSat_ew) { + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Saturation_UpperSat_ew; + } else if (AutopilotLaws_DWork.DelayInput1_DSTATE < AutopilotLaws_P.Saturation_LowerSat_an) { + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Saturation_LowerSat_an; + } + + rtb_Add3_lz = (rtb_Vz - rtb_Max1) * AutopilotLaws_P.ftmintoms_Gain_j / AutopilotLaws_DWork.DelayInput1_DSTATE; + if (R > 1.0) { + R = 1.0; + } else if (R < -1.0) { + R = -1.0; + } + + if (rtb_Add3_lz > 1.0) { + rtb_Add3_lz = 1.0; + } else if (rtb_Add3_lz < -1.0) { + rtb_Add3_lz = -1.0; + } + + rtb_Sum1_g = AutopilotLaws_P.Gain_Gain_gr * std::asin(R) * AutopilotLaws_P.Gain1_Gain_ml + + AutopilotLaws_P.Gain_Gain_by * std::asin(rtb_Add3_lz) * AutopilotLaws_P.Gain2_Gain_m; + rtb_uDLookupTable_m = look1_binlxpw(AutopilotLaws_U.in.data.total_weight_kg, AutopilotLaws_P.uDLookupTable_bp01Data, + AutopilotLaws_P.uDLookupTable_tableData, 3U); + rtb_Sum_es = AutopilotLaws_P.Constant1_Value_o0 - rtb_GainTheta; + rtb_Sum3_m3 = AutopilotLaws_P.Constant2_Value_kz - AutopilotLaws_U.in.data.H_ind_ft; + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.fpmtoms_Gain_po * AutopilotLaws_U.in.data.H_dot_ft_min; + rtb_Add3_lz = AutopilotLaws_P.kntoms_Gain_bh * AutopilotLaws_U.in.data.V_gnd_kn; + if (rtb_Add3_lz > AutopilotLaws_P.Saturation_UpperSat_pd) { + rtb_Add3_lz = AutopilotLaws_P.Saturation_UpperSat_pd; + } else if (rtb_Add3_lz < AutopilotLaws_P.Saturation_LowerSat_l) { + rtb_Add3_lz = AutopilotLaws_P.Saturation_LowerSat_l; + } + + AutopilotLaws_DWork.DelayInput1_DSTATE = std::atan(AutopilotLaws_DWork.DelayInput1_DSTATE / rtb_Add3_lz) * + AutopilotLaws_P.Gain_Gain_cr; + R = AutopilotLaws_P.Gain1_Gain_ga * AutopilotLaws_DWork.DelayInput1_DSTATE; + rtb_Gain1_na = AutopilotLaws_P.Gain1_Gain_hm2 * rtb_GainTheta1; + rtb_Add3_lz = AutopilotLaws_P.kntoms_Gain_py * AutopilotLaws_U.in.data.V_gnd_kn; + if (rtb_Add3_lz > AutopilotLaws_P.Saturation_UpperSat_ec) { + rtb_Add3_lz = AutopilotLaws_P.Saturation_UpperSat_ec; + } else if (rtb_Add3_lz < AutopilotLaws_P.Saturation_LowerSat_m) { + rtb_Add3_lz = AutopilotLaws_P.Saturation_LowerSat_m; + } + + rtb_GainTheta1 = (AutopilotLaws_P.Gain1_Gain_ol * rtb_GainTheta - std::atan(AutopilotLaws_P.fpmtoms_Gain_k * + AutopilotLaws_U.in.data.H_dot_ft_min / rtb_Add3_lz) * AutopilotLaws_P.Gain_Gain_hc * AutopilotLaws_P.Gain1_Gain_ln) * + (AutopilotLaws_P.Constant_Value_h - std::cos(rtb_Gain1_na)); + rtb_Add3_lz = std::sin(rtb_Gain1_na); + rtb_Sum_kq = AutopilotLaws_P.Gain1_Gain_it * AutopilotLaws_U.in.data.Psi_magnetic_track_deg; + rtb_Gain1_na = AutopilotLaws_P.ktstomps_Gain_k5 * AutopilotLaws_U.in.data.V_gnd_kn; + AutopilotLaws_WashoutFilter(AutopilotLaws_P._Gain_f * (AutopilotLaws_P.GStoGS_CAS_Gain_j * rtb_Gain1_na), + AutopilotLaws_P.WashoutFilter_C1_cn, AutopilotLaws_U.in.time.dt, &rtb_Gain1_na, + &AutopilotLaws_DWork.sf_WashoutFilter_i); + AutopilotLaws_LeadLagFilter(rtb_Gain1_na - AutopilotLaws_P.g_Gain_p * (AutopilotLaws_P.Gain1_Gain_mxw * + (AutopilotLaws_P.Gain_Gain_er * (rtb_GainTheta1 + rtb_Add3_lz * std::sin(rtb_Sum_kq - AutopilotLaws_P.Gain1_Gain_a * + AutopilotLaws_U.in.data.Psi_magnetic_deg)))), AutopilotLaws_P.HighPassFilter_C1_gw, + AutopilotLaws_P.HighPassFilter_C2_e, AutopilotLaws_P.HighPassFilter_C3_di, AutopilotLaws_P.HighPassFilter_C4_a, + AutopilotLaws_U.in.time.dt, &rtb_Sum_kq, &AutopilotLaws_DWork.sf_LeadLagFilter_g); + AutopilotLaws_LeadLagFilter(AutopilotLaws_P.ktstomps_Gain_mf * AutopilotLaws_U.in.data.V_ias_kn, + AutopilotLaws_P.LowPassFilter_C1_d1, AutopilotLaws_P.LowPassFilter_C2_e, AutopilotLaws_P.LowPassFilter_C3_l, + AutopilotLaws_P.LowPassFilter_C4_a, AutopilotLaws_U.in.time.dt, &rtb_Gain1_na, + &AutopilotLaws_DWork.sf_LeadLagFilter_n); + rtb_Gain1_na = (rtb_Sum_kq + rtb_Gain1_na) * AutopilotLaws_P.ug_Gain_c; + rtb_GainTheta1 = (AutopilotLaws_P.Gain1_Gain_nc * rtb_Gain1_na + R) * AutopilotLaws_P.Gain_Gain_bg; + rtb_Sum_kq = (AutopilotLaws_U.in.data.V_ias_kn - AutopilotLaws_U.in.input.V_c_kn) * AutopilotLaws_P.Gain1_Gain_ke; + rtb_Delay_j = ((rtb_Sum3_m3 > AutopilotLaws_P.CompareToConstant6_const_d) && (rtb_GainTheta1 < + AutopilotLaws_P.CompareToConstant5_const_h) && (rtb_Sum_kq < AutopilotLaws_P.CompareToConstant2_const_j) && (rtb_dme + == AutopilotLaws_P.CompareToConstant8_const)); + R += rtb_Gain1_na; + if (rtb_Delay_j) { + rtb_Gain1_na = AutopilotLaws_P.Constant_Value_o; + } else { + if (rtb_Sum3_m3 > AutopilotLaws_P.CompareToConstant_const_h) { + rtb_Gain1_na = AutopilotLaws_P.Constant1_Value_g5; + } else { + rtb_Gain1_na = AutopilotLaws_P.Gain5_Gain_n * rtb_GainTheta1; + } + + if (rtb_Sum_kq <= rtb_Gain1_na) { + if (rtb_Sum3_m3 > AutopilotLaws_P.CompareToConstant4_const_e) { + rtb_Gain1_na = std::fmax(AutopilotLaws_P.Constant2_Value_m, AutopilotLaws_P.Gain1_Gain_m * rtb_GainTheta1); + } else { + rtb_Gain1_na = AutopilotLaws_P.Gain6_Gain_fa * rtb_GainTheta1; + } + + if (rtb_Sum_kq >= rtb_Gain1_na) { + rtb_Gain1_na = rtb_Sum_kq; + } + } + } + + rtb_Sum2_j = (AutopilotLaws_P.Gain_Gain_c2 * R - AutopilotLaws_DWork.DelayInput1_DSTATE) + rtb_Gain1_na; + rtb_Add3_lz = AutopilotLaws_P.kntoms_Gain_om * AutopilotLaws_U.in.data.V_tas_kn; + if (rtb_Sum3_m3 < 0.0) { + rtb_Sum_kq = -1.0; + } else { + rtb_Sum_kq = (rtb_Sum3_m3 > 0.0); + } + + if (rtb_Add3_lz > AutopilotLaws_P.Saturation_UpperSat_ed) { + rtb_Add3_lz = AutopilotLaws_P.Saturation_UpperSat_ed; + } else if (rtb_Add3_lz < AutopilotLaws_P.Saturation_LowerSat_ee) { + rtb_Add3_lz = AutopilotLaws_P.Saturation_LowerSat_ee; + } + + rtb_Add3_lz = (rtb_Sum_kq * AutopilotLaws_P.Constant3_Value_ew - AutopilotLaws_U.in.data.H_dot_ft_min) * + AutopilotLaws_P.ftmintoms_Gain_m / rtb_Add3_lz; + if (rtb_Add3_lz > 1.0) { + rtb_Add3_lz = 1.0; + } else if (rtb_Add3_lz < -1.0) { + rtb_Add3_lz = -1.0; + } + + rtb_Gain_pp = AutopilotLaws_P.Gain_Gain_kon * std::asin(rtb_Add3_lz); + rtb_Gain1_na = AutopilotLaws_P.kntoms_Gain_iw * AutopilotLaws_U.in.data.V_tas_kn; + if (rtb_Gain1_na > AutopilotLaws_P.Saturation_UpperSat_jt) { + rtb_Gain1_na = AutopilotLaws_P.Saturation_UpperSat_jt; + } else if (rtb_Gain1_na < AutopilotLaws_P.Saturation_LowerSat_ih) { + rtb_Gain1_na = AutopilotLaws_P.Saturation_LowerSat_ih; + } + + rtb_Add3_lz = (AutopilotLaws_P.Constant_Value_iaf - AutopilotLaws_U.in.data.H_dot_ft_min) * + AutopilotLaws_P.ftmintoms_Gain_lv / rtb_Gain1_na; + if (rtb_Add3_lz > 1.0) { + rtb_Add3_lz = 1.0; + } else if (rtb_Add3_lz < -1.0) { + rtb_Add3_lz = -1.0; + } + + rtb_Gain_ib = AutopilotLaws_P.Gain_Gain_o1 * std::asin(rtb_Add3_lz); + if (rtb_Delay_j) { + rtb_Sum_kq = rtb_Sum2_j; + } else if (rtb_Sum3_m3 > AutopilotLaws_P.Switch_Threshold_k) { + rtb_Sum_kq = std::fmax(rtb_Sum2_j, rtb_Gain_pp); + } else { + rtb_Sum_kq = std::fmin(rtb_Sum2_j, rtb_Gain_pp); + } + + AutopilotLaws_Voter1(rtb_Sum_es, rtb_Sum_kq, rtb_Gain_ib, &R); + AutopilotLaws_LagFilter(AutopilotLaws_U.in.input.H_c_ft - AutopilotLaws_U.in.data.H_ft, AutopilotLaws_P.LagFilter_C1_b, + AutopilotLaws_U.in.time.dt, &rtb_Gain1_na, &AutopilotLaws_DWork.sf_LagFilter_d); + rtb_Add3_lz = AutopilotLaws_P.Gain2_Gain_hq * rtb_Gain1_na; + rtb_GainTheta1 = AutopilotLaws_P.kntoms_Gain_j * AutopilotLaws_U.in.data.V_tas_kn; + if (rtb_Add3_lz > AutopilotLaws_P.Saturation_UpperSat_f3) { + rtb_Add3_lz = AutopilotLaws_P.Saturation_UpperSat_f3; + } else if (rtb_Add3_lz < AutopilotLaws_P.Saturation_LowerSat_b) { + rtb_Add3_lz = AutopilotLaws_P.Saturation_LowerSat_b; + } + + if (rtb_GainTheta1 > AutopilotLaws_P.Saturation_UpperSat_nuy) { + rtb_GainTheta1 = AutopilotLaws_P.Saturation_UpperSat_nuy; + } else if (rtb_GainTheta1 < AutopilotLaws_P.Saturation_LowerSat_dj) { + rtb_GainTheta1 = AutopilotLaws_P.Saturation_LowerSat_dj; + } + + rtb_Add3_lz = ((AutopilotLaws_U.in.input.H_dot_c_fpm + rtb_Add3_lz) - AutopilotLaws_U.in.data.H_dot_ft_min) * + AutopilotLaws_P.ftmintoms_Gain_kr / rtb_GainTheta1; + if (rtb_Add3_lz > 1.0) { + rtb_Add3_lz = 1.0; + } else if (rtb_Add3_lz < -1.0) { + rtb_Add3_lz = -1.0; + } + + rtb_GainTheta1 = AutopilotLaws_P.Gain_Gain_fs * std::asin(rtb_Add3_lz); + switch (static_cast(rtb_dme)) { + case 0: + b_L = AutopilotLaws_P.Constant_Value_dh; + break; + + case 1: + b_L = Phi2; + break; + + case 2: + break; + + case 3: + if (rtb_Compare_jy) { + b_L = rtb_Sum_i; + } else if (distance_m > AutopilotLaws_P.Switch_Threshold) { + b_L = std::fmax(rtb_Sum_i, rtb_Gain_ml); + } else { + b_L = std::fmin(rtb_Sum_i, rtb_Gain_ml); + } + break; + + case 4: + b_L = rtb_Cos1_pk; + break; + + case 5: + b_L = rtb_FD_i; + break; + + case 6: + b_L = AutopilotLaws_P.Gain1_Gain_d * rtb_Product_dh; + break; + + case 7: + if (rtb_on_ground > AutopilotLaws_P.Switch1_Threshold_j) { + b_L = AutopilotLaws_P.Gain2_Gain_h * rtb_Gain4; + } else { + b_L = ((AutopilotLaws_P.Gain1_Gain_i * rtb_Y_i0 + rtb_Y_i) + rtb_Sum1_g * rtb_uDLookupTable_m) * + AutopilotLaws_P.Gain6_Gain_f; + } + break; + + case 8: + b_L = R; + break; + + default: + b_L = rtb_GainTheta1; + break; + } + + if (b_L > AutopilotLaws_P.Constant1_Value_i) { + rtb_Gain1_na = AutopilotLaws_P.Constant1_Value_i; + } else { + rtb_Gain1_na = AutopilotLaws_P.Gain1_Gain_n * AutopilotLaws_P.Constant1_Value_i; + if (b_L >= rtb_Gain1_na) { + rtb_Gain1_na = b_L; + } + } + + AutopilotLaws_RateLimiter(rtb_Gain1_na - rtb_error_c, AutopilotLaws_P.RateLimiterVariableTs1_up, + AutopilotLaws_P.RateLimiterVariableTs1_lo, AutopilotLaws_U.in.time.dt, + AutopilotLaws_P.RateLimiterVariableTs1_InitialCondition, &rtb_Gain1_na, &AutopilotLaws_DWork.sf_RateLimiter_h); + AutopilotLaws_LagFilter(rtb_Gain1_na, AutopilotLaws_P.LagFilter_C1_gh, AutopilotLaws_U.in.time.dt, &R, + &AutopilotLaws_DWork.sf_LagFilter_pe); + AutopilotLaws_DWork.icLoad_f = ((rtb_fpmtoms == 0) || AutopilotLaws_DWork.icLoad_f); + if (AutopilotLaws_DWork.icLoad_f) { + AutopilotLaws_DWork.Delay_DSTATE_h2 = rtb_GainTheta; + } + + AutopilotLaws_VSLimiter(AutopilotLaws_P.VS_Gain_n * Phi2, &AutopilotLaws_Y.out, &rtb_Gain1_na); + if (!rtb_Compare_jy) { + if (distance_m > AutopilotLaws_P.Switch_Threshold_h) { + rtb_Sum_i = std::fmax(rtb_Sum_i, AutopilotLaws_P.VS_Gain_j * rtb_Gain_ml); + } else { + rtb_Sum_i = std::fmin(rtb_Sum_i, AutopilotLaws_P.VS_Gain_j * rtb_Gain_ml); + } + } + + AutopilotLaws_VSLimiter(AutopilotLaws_P.Gain_Gain_k2 * rtb_Sum_i, &AutopilotLaws_Y.out, &distance_m); + AutopilotLaws_VSLimiter_f(rtb_Product_dh, &AutopilotLaws_Y.out, &rtb_Sum_i); + Phi2 = AutopilotLaws_P.Gain3_Gain_l * rtb_Y_i0; + b_L = AutopilotLaws_P.VS_Gain_e * rtb_Sum1_g; + AutopilotLaws_WashoutFilter(rtb_Saturation, AutopilotLaws_P.WashoutFilter1_C1_h, AutopilotLaws_U.in.time.dt, + &rtb_error_c, &AutopilotLaws_DWork.sf_WashoutFilter_k); + rtb_Add3_lz = std::abs(rtb_error_c); + if (rtb_Add3_lz > AutopilotLaws_P.Saturation_UpperSat_ig) { + rtb_Add3_lz = AutopilotLaws_P.Saturation_UpperSat_ig; + } else if (rtb_Add3_lz < AutopilotLaws_P.Saturation_LowerSat_ous) { + rtb_Add3_lz = AutopilotLaws_P.Saturation_LowerSat_ous; + } + + rtb_Saturation = AutopilotLaws_P.Gain_Gain_j0 * rtb_Add3_lz; + if (rtb_on_ground <= AutopilotLaws_P.Switch_Threshold_c) { + rtb_Gain4 = ((rtb_Y_i + Phi2) + rtb_uDLookupTable_m * b_L) + rtb_Saturation; + } + + AutopilotLaws_VSLimiter_f(rtb_Gain4, &AutopilotLaws_Y.out, &rtb_error_c); + if (!rtb_Delay_j) { + if (rtb_Sum3_m3 > AutopilotLaws_P.Switch_Threshold_hz) { + rtb_Sum2_j = std::fmax(rtb_Sum2_j, AutopilotLaws_P.VS_Gain_n5 * rtb_Gain_pp); + } else { + rtb_Sum2_j = std::fmin(rtb_Sum2_j, AutopilotLaws_P.VS_Gain_n5 * rtb_Gain_pp); + } + } + + AutopilotLaws_Voter1(rtb_Sum_es, AutopilotLaws_P.Gain_Gain_o2 * rtb_Sum2_j, AutopilotLaws_P.VS_Gain_nx * rtb_Gain_ib, + &rtb_Gain_ml); + AutopilotLaws_VSLimiter(rtb_Gain_ml, &AutopilotLaws_Y.out, &rtb_Cos1_pk); + if (AutopilotLaws_U.in.input.vertical_mode == 24.0) { + rtb_Sum_kq = 0.15; + } else { + rtb_Sum_kq = 0.1; + } + + rtb_Y_i0 = rtb_lo_f * rtb_Sum_kq * 57.295779513082323; + switch (static_cast(rtb_dme)) { + case 0: + rtb_Gain1_na = AutopilotLaws_P.Constant_Value_dh; + break; + + case 1: + break; + + case 2: + rtb_Gain1_na = L; + break; + + case 3: + rtb_Gain1_na = distance_m; + break; + + case 4: + rtb_Gain1_na = rtb_Cos_i; + break; + + case 5: + rtb_Gain1_na = rtb_Add3_j4; + break; + + case 6: + rtb_Gain1_na = rtb_Sum_i; + break; + + case 7: + rtb_Gain1_na = rtb_error_c; + break; + + case 8: + rtb_Gain1_na = rtb_Cos1_pk; + break; + + default: + rtb_Gain1_na = std::fmax(-rtb_Y_i0, std::fmin(rtb_Y_i0, AutopilotLaws_P.VS_Gain_ne * rtb_GainTheta1)); + break; + } + + rtb_Gain1_na += rtb_GainTheta; + if (rtb_Gain1_na > AutopilotLaws_P.Constant1_Value_i) { + rtb_Gain1_na = AutopilotLaws_P.Constant1_Value_i; + } else { + rtb_GainTheta1 = AutopilotLaws_P.Gain1_Gain_m4 * AutopilotLaws_P.Constant1_Value_i; + if (rtb_Gain1_na < rtb_GainTheta1) { + rtb_Gain1_na = rtb_GainTheta1; + } + } + + rtb_GainTheta1 = rtb_lo_f * 0.3 * 57.295779513082323; + AutopilotLaws_DWork.DelayInput1_DSTATE = rtb_GainTheta1 * AutopilotLaws_U.in.time.dt; + rtb_Gain1_na = std::fmin(rtb_Gain1_na - AutopilotLaws_DWork.Delay_DSTATE_h2, AutopilotLaws_DWork.DelayInput1_DSTATE); + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Gain1_Gain_i0 * rtb_GainTheta1; + AutopilotLaws_DWork.DelayInput1_DSTATE *= AutopilotLaws_U.in.time.dt; + AutopilotLaws_DWork.Delay_DSTATE_h2 += std::fmax(rtb_Gain1_na, AutopilotLaws_DWork.DelayInput1_DSTATE); + AutopilotLaws_LagFilter(AutopilotLaws_DWork.Delay_DSTATE_h2, AutopilotLaws_P.LagFilter_C1_i, + AutopilotLaws_U.in.time.dt, &rtb_error_c, &AutopilotLaws_DWork.sf_LagFilter_gn); + AutopilotLaws_RateLimiter(static_cast(rtb_fpmtoms), AutopilotLaws_P.RateLimiterVariableTs_up_i, + AutopilotLaws_P.RateLimiterVariableTs_lo_o, AutopilotLaws_U.in.time.dt, + AutopilotLaws_P.RateLimiterVariableTs_InitialCondition_p, &rtb_Gain1_na, &AutopilotLaws_DWork.sf_RateLimiter_eb); + if (rtb_Gain1_na > AutopilotLaws_P.Saturation_UpperSat_ix) { + rtb_Gain1_na = AutopilotLaws_P.Saturation_UpperSat_ix; + } else if (rtb_Gain1_na < AutopilotLaws_P.Saturation_LowerSat_eq) { + rtb_Gain1_na = AutopilotLaws_P.Saturation_LowerSat_eq; + } + + rtb_GainTheta1 = rtb_error_c * rtb_Gain1_na; + AutopilotLaws_LagFilter(rtb_Y_j, AutopilotLaws_P.LagFilter1_C1_d, AutopilotLaws_U.in.time.dt, &rtb_error_c, + &AutopilotLaws_DWork.sf_LagFilter_cs); + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.Gain7_Gain_l * rtb_error_c; + AutopilotLaws_Y.out.output.flight_director.Theta_c_deg = R; + AutopilotLaws_Y.out.output.autopilot.Theta_c_deg = (AutopilotLaws_P.Constant_Value_i4 - rtb_Gain1_na) * rtb_GainTheta + + rtb_GainTheta1; + AutopilotLaws_Y.out.output.flare_law.condition_Flare = ((AutopilotLaws_U.in.data.H_radio_ft < 60.0) && + ((AutopilotLaws_U.in.data.H_radio_ft * 15.0 <= std::abs(std::fmax(AutopilotLaws_DWork.DelayInput1_DSTATE, a))) || + (AutopilotLaws_U.in.data.H_radio_ft <= 45.0))); + AutopilotLaws_Y.out.output.flare_law.H_dot_radio_fpm = rtb_Max1; + AutopilotLaws_Y.out.output.flare_law.H_dot_c_fpm = rtb_Vz; + AutopilotLaws_Y.out.output.flare_law.delta_Theta_H_dot_deg = b_L; + AutopilotLaws_Y.out.output.flare_law.delta_Theta_bz_deg = rtb_Y_i; + AutopilotLaws_Y.out.output.flare_law.delta_Theta_bx_deg = Phi2; + AutopilotLaws_Y.out.output.flare_law.delta_Theta_beta_c_deg = rtb_Saturation; + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_U.in.data.altimeter_setting_left_mbar; + AutopilotLaws_DWork.DelayInput1_DSTATE_g = AutopilotLaws_U.in.data.altimeter_setting_right_mbar; + AutopilotLaws_DWork.Delay_DSTATE = b_R; + for (rtb_fpmtoms = 0; rtb_fpmtoms < 99; rtb_fpmtoms++) { + AutopilotLaws_DWork.Delay_DSTATE_l[rtb_fpmtoms] = AutopilotLaws_DWork.Delay_DSTATE_l[rtb_fpmtoms + 1]; + AutopilotLaws_DWork.Delay_DSTATE_h5[rtb_fpmtoms] = AutopilotLaws_DWork.Delay_DSTATE_h5[rtb_fpmtoms + 1]; + } + + AutopilotLaws_DWork.Delay_DSTATE_l[99] = rtb_valid; + AutopilotLaws_DWork.Delay_DSTATE_h5[99] = rtb_valid_m; + AutopilotLaws_DWork.icLoad = false; + AutopilotLaws_DWork.Delay_DSTATE_c = rtb_Add3_np; + AutopilotLaws_DWork.Delay_DSTATE_b = rtb_Cos1_j; + AutopilotLaws_DWork.icLoad_f = false; +} + +void AutopilotLawsModelClass::initialize() +{ + { + real_T rtb_out_ff; + AutopilotLaws_DWork.DelayInput1_DSTATE = AutopilotLaws_P.DetectChange_vinit; + AutopilotLaws_DWork.DelayInput1_DSTATE_g = AutopilotLaws_P.DetectChange1_vinit; + AutopilotLaws_DWork.Delay_DSTATE = AutopilotLaws_P.DiscreteDerivativeVariableTs_InitialCondition; + for (int32_T i{0}; i < 100; i++) { + AutopilotLaws_DWork.Delay_DSTATE_l[i] = AutopilotLaws_P.Delay_InitialCondition; + AutopilotLaws_DWork.Delay_DSTATE_h5[i] = AutopilotLaws_P.Delay_InitialCondition_b; + } + + AutopilotLaws_DWork.Delay_DSTATE_h = AutopilotLaws_P.DiscreteTimeIntegratorVariableTs_InitialCondition; + AutopilotLaws_DWork.icLoad = true; + AutopilotLaws_DWork.Delay_DSTATE_c = AutopilotLaws_P.DiscreteDerivativeVariableTs1_InitialCondition; + AutopilotLaws_DWork.Delay_DSTATE_b = AutopilotLaws_P.DiscreteDerivativeVariableTs_InitialCondition_f; + AutopilotLaws_DWork.icLoad_f = true; + AutopilotLaws_Chart_Init(&rtb_out_ff); + AutopilotLaws_MATLABFunction_f_Init(&AutopilotLaws_DWork.sf_MATLABFunction_m); + AutopilotLaws_Chart_Init(&rtb_out_ff); + AutopilotLaws_MATLABFunction_f_Init(&AutopilotLaws_DWork.sf_MATLABFunction_e); + AutopilotLaws_Chart_g_Init(&rtb_out_ff); + AutopilotLaws_Chart_g_Init(&rtb_out_ff); + AutopilotLaws_B.u = AutopilotLaws_P.Y_Y0; + AutopilotLaws_DWork.k = 5.0; + AutopilotLaws_DWork.maxH_dot = 1500.0; + } +} + +void AutopilotLawsModelClass::terminate() +{ +} + +AutopilotLawsModelClass::AutopilotLawsModelClass(): + AutopilotLaws_U(), + AutopilotLaws_Y(), + AutopilotLaws_B(), + AutopilotLaws_DWork() +{ +} + +AutopilotLawsModelClass::~AutopilotLawsModelClass() +{ +} diff --git a/hdw-a339x/src/wasm/fbw_a320/src/model/AutopilotLaws.h b/hdw-a339x/src/wasm/fbw_a320/src/model/AutopilotLaws.h new file mode 100644 index 000000000..971994da6 --- /dev/null +++ b/hdw-a339x/src/wasm/fbw_a320/src/model/AutopilotLaws.h @@ -0,0 +1,991 @@ +#ifndef RTW_HEADER_AutopilotLaws_h_ +#define RTW_HEADER_AutopilotLaws_h_ +#include "rtwtypes.h" +#include "AutopilotLaws_types.h" + +class AutopilotLawsModelClass final +{ + public: + struct rtDW_LagFilter_AutopilotLaws_T { + real_T pY; + real_T pU; + boolean_T pY_not_empty; + boolean_T pU_not_empty; + }; + + struct rtDW_RateLimiter_AutopilotLaws_T { + real_T pY; + boolean_T pY_not_empty; + }; + + struct rtDW_MATLABFunction_AutopilotLaws_d_T { + real_T limit; + real_T lastPsi; + real_T limitDeltaPsi; + boolean_T lastPsi_not_empty; + }; + + struct rtDW_Chart_AutopilotLaws_T { + uint8_T is_active_c10_AutopilotLaws; + uint8_T is_c10_AutopilotLaws; + }; + + struct rtDW_RateLimiter_AutopilotLaws_l_T { + real_T pY; + boolean_T pY_not_empty; + }; + + struct rtDW_Chart_AutopilotLaws_m_T { + uint8_T is_active_c15_AutopilotLaws; + uint8_T is_c15_AutopilotLaws; + }; + + struct rtDW_storevalue_AutopilotLaws_T { + real_T storage; + boolean_T storage_not_empty; + }; + + struct rtDW_LeadLagFilter_AutopilotLaws_T { + real_T pY; + real_T pU; + boolean_T pY_not_empty; + boolean_T pU_not_empty; + }; + + struct rtDW_WashoutFilter_AutopilotLaws_T { + real_T pY; + real_T pU; + boolean_T pY_not_empty; + boolean_T pU_not_empty; + }; + + struct BlockIO_AutopilotLaws_T { + real_T u; + }; + + struct D_Work_AutopilotLaws_T { + real_T DelayInput1_DSTATE; + real_T DelayInput1_DSTATE_g; + real_T Delay_DSTATE; + real_T Delay_DSTATE_h; + real_T Delay_DSTATE_hc; + real_T Delay_DSTATE_c; + real_T Delay_DSTATE_b; + real_T Delay_DSTATE_h2; + real_T prevVerticalLaw; + real_T prevTarget; + real_T prevVerticalLaw_g; + real_T prevTarget_f; + real_T Tau; + real_T H_bias; + real_T dH_offset; + real_T k; + real_T maxH_dot; + real_T nav_gs_deg; + real_T pY; + real_T pY_e; + real_T limit; + boolean_T Delay_DSTATE_l[100]; + boolean_T Delay_DSTATE_h5[100]; + uint8_T is_active_c5_AutopilotLaws; + uint8_T is_c5_AutopilotLaws; + boolean_T icLoad; + boolean_T icLoad_f; + boolean_T prevVerticalLaw_not_empty; + boolean_T prevTarget_not_empty; + boolean_T islevelOffActive; + boolean_T prevVerticalLaw_not_empty_o; + boolean_T prevTarget_not_empty_e; + boolean_T islevelOffActive_b; + boolean_T wasActive; + boolean_T wasActive_not_empty; + boolean_T wasActive_o; + boolean_T wasActive_not_empty_m; + boolean_T nav_gs_deg_not_empty; + boolean_T pY_not_empty; + boolean_T pY_not_empty_j; + boolean_T limit_not_empty; + rtDW_WashoutFilter_AutopilotLaws_T sf_WashoutFilter_j; + rtDW_LeadLagFilter_AutopilotLaws_T sf_LeadLagFilter_p; + rtDW_LeadLagFilter_AutopilotLaws_T sf_LeadLagFilter_c; + rtDW_WashoutFilter_AutopilotLaws_T sf_WashoutFilter_fs; + rtDW_LeadLagFilter_AutopilotLaws_T sf_LeadLagFilter_kq; + rtDW_LeadLagFilter_AutopilotLaws_T sf_LeadLagFilter_b; + rtDW_LagFilter_AutopilotLaws_T sf_LagFilter_d; + rtDW_RateLimiter_AutopilotLaws_T sf_RateLimiter_h; + rtDW_RateLimiter_AutopilotLaws_T sf_RateLimiter_eb; + rtDW_WashoutFilter_AutopilotLaws_T sf_WashoutFilter_f; + rtDW_LagFilter_AutopilotLaws_T sf_LagFilter_gn; + rtDW_LagFilter_AutopilotLaws_T sf_LagFilter_pe; + rtDW_WashoutFilter_AutopilotLaws_T sf_WashoutFilter_i; + rtDW_LeadLagFilter_AutopilotLaws_T sf_LeadLagFilter_n; + rtDW_LeadLagFilter_AutopilotLaws_T sf_LeadLagFilter_g; + rtDW_WashoutFilter_AutopilotLaws_T sf_WashoutFilter_n; + rtDW_LeadLagFilter_AutopilotLaws_T sf_LeadLagFilter_ja; + rtDW_LeadLagFilter_AutopilotLaws_T sf_LeadLagFilter_es; + rtDW_storevalue_AutopilotLaws_T sf_storevalue_g; + rtDW_RateLimiter_AutopilotLaws_l_T sf_RateLimiter_l; + rtDW_WashoutFilter_AutopilotLaws_T sf_WashoutFilter_db; + rtDW_LagFilter_AutopilotLaws_T sf_LagFilter_i; + rtDW_LagFilter_AutopilotLaws_T sf_LagFilter_l; + rtDW_LagFilter_AutopilotLaws_T sf_LagFilter_f; + rtDW_LagFilter_AutopilotLaws_T sf_LagFilter_gx; + rtDW_LagFilter_AutopilotLaws_T sf_LagFilter_cf; + rtDW_LagFilter_AutopilotLaws_T sf_LagFilter_p; + rtDW_WashoutFilter_AutopilotLaws_T sf_WashoutFilter_g5; + rtDW_LeadLagFilter_AutopilotLaws_T sf_LeadLagFilter_a; + rtDW_LeadLagFilter_AutopilotLaws_T sf_LeadLagFilter_j; + rtDW_WashoutFilter_AutopilotLaws_T sf_WashoutFilter_h; + rtDW_LeadLagFilter_AutopilotLaws_T sf_LeadLagFilter_kp; + rtDW_LeadLagFilter_AutopilotLaws_T sf_LeadLagFilter_e; + rtDW_WashoutFilter_AutopilotLaws_T sf_WashoutFilter_k; + rtDW_WashoutFilter_AutopilotLaws_T sf_WashoutFilter_g; + rtDW_LeadLagFilter_AutopilotLaws_T sf_LeadLagFilter_hp; + rtDW_LeadLagFilter_AutopilotLaws_T sf_LeadLagFilter_k; + rtDW_LagFilter_AutopilotLaws_T sf_LagFilter_cs; + rtDW_LagFilter_AutopilotLaws_T sf_LagFilter_a; + rtDW_LagFilter_AutopilotLaws_T sf_LagFilter_g; + rtDW_WashoutFilter_AutopilotLaws_T sf_WashoutFilter_d; + rtDW_LeadLagFilter_AutopilotLaws_T sf_LeadLagFilter_m; + rtDW_LeadLagFilter_AutopilotLaws_T sf_LeadLagFilter_h; + rtDW_WashoutFilter_AutopilotLaws_T sf_WashoutFilter; + rtDW_LeadLagFilter_AutopilotLaws_T sf_LeadLagFilter_o; + rtDW_LeadLagFilter_AutopilotLaws_T sf_LeadLagFilter; + rtDW_Chart_AutopilotLaws_T sf_Chart_ba; + rtDW_MATLABFunction_AutopilotLaws_d_T sf_MATLABFunction_e; + rtDW_RateLimiter_AutopilotLaws_T sf_RateLimiter_d; + rtDW_LagFilter_AutopilotLaws_T sf_LagFilter_o; + rtDW_LagFilter_AutopilotLaws_T sf_LagFilter_mp; + rtDW_storevalue_AutopilotLaws_T sf_storevalue; + rtDW_Chart_AutopilotLaws_m_T sf_Chart_b; + rtDW_RateLimiter_AutopilotLaws_l_T sf_RateLimiter_e; + rtDW_LagFilter_AutopilotLaws_T sf_LagFilter_h2; + rtDW_Chart_AutopilotLaws_m_T sf_Chart_h; + rtDW_RateLimiter_AutopilotLaws_l_T sf_RateLimiter_n; + rtDW_LagFilter_AutopilotLaws_T sf_LagFilter_h; + rtDW_LagFilter_AutopilotLaws_T sf_LagFilter_c; + rtDW_LagFilter_AutopilotLaws_T sf_LagFilter_m; + rtDW_Chart_AutopilotLaws_T sf_Chart; + rtDW_MATLABFunction_AutopilotLaws_d_T sf_MATLABFunction_m; + rtDW_RateLimiter_AutopilotLaws_T sf_RateLimiter; + rtDW_LagFilter_AutopilotLaws_T sf_LagFilter; + }; + + struct ExternalInputs_AutopilotLaws_T { + ap_laws_input in; + }; + + struct ExternalOutputs_AutopilotLaws_T { + ap_laws_output out; + }; + + struct Parameters_AutopilotLaws_T { + ap_laws_output ap_laws_output_MATLABStruct; + real_T ScheduledGain2_BreakpointsForDimension1[7]; + real_T ScheduledGain_BreakpointsForDimension1[6]; + real_T ScheduledGain_BreakpointsForDimension1_j[3]; + real_T ScheduledGain_BreakpointsForDimension1_h[7]; + real_T ScheduledGain_BreakpointsForDimension1_o[7]; + real_T ScheduledGain_BreakpointsForDimension1_a[8]; + real_T ScheduledGain3_BreakpointsForDimension1[5]; + real_T ScheduledGain2_BreakpointsForDimension1_h[8]; + real_T ScheduledGain1_BreakpointsForDimension1[7]; + real_T LagFilter2_C1; + real_T LagFilter_C1; + real_T LagFilter_C1_g; + real_T LagFilter_C1_a; + real_T LagFilter1_C1; + real_T LagFilter_C1_k; + real_T LagFilter_C1_l; + real_T WashoutFilter_C1; + real_T LagFilter_C1_ai; + real_T WashoutFilter_C1_e; + real_T HighPassFilter_C1; + real_T LowPassFilter_C1; + real_T WashoutFilter_C1_e4; + real_T HighPassFilter_C1_e; + real_T LowPassFilter_C1_n; + real_T WashoutFilter_C1_k; + real_T HighPassFilter_C1_b; + real_T LowPassFilter_C1_d; + real_T WashoutFilter_C1_o; + real_T HighPassFilter_C1_g; + real_T LowPassFilter_C1_m; + real_T WashoutFilter_C1_p; + real_T HighPassFilter_C1_n; + real_T LowPassFilter_C1_l; + real_T WashoutFilter_C1_j; + real_T HighPassFilter_C1_i; + real_T LowPassFilter_C1_l4; + real_T WashoutFilter_C1_c; + real_T HighPassFilter_C1_d; + real_T LowPassFilter_C1_e; + real_T LagFilter2_C1_d; + real_T LagFilter3_C1; + real_T LagFilter4_C1; + real_T WashoutFilter1_C1; + real_T LagFilter1_C1_l; + real_T LagFilter2_C1_e; + real_T LagFilter_C1_d; + real_T WashoutFilter_C1_m; + real_T LagFilterH_C1; + real_T LeadLagFilter_C1; + real_T LeadLagFilter_C1_a; + real_T WashoutFilter_C1_cn; + real_T HighPassFilter_C1_gw; + real_T LowPassFilter_C1_d1; + real_T LagFilter_C1_b; + real_T LagFilter_C1_gh; + real_T WashoutFilter1_C1_h; + real_T LagFilter_C1_i; + real_T LagFilter1_C1_d; + real_T HighPassFilter_C2; + real_T LowPassFilter_C2; + real_T HighPassFilter_C2_c; + real_T LowPassFilter_C2_a; + real_T HighPassFilter_C2_g; + real_T LowPassFilter_C2_p; + real_T HighPassFilter_C2_l; + real_T LowPassFilter_C2_l; + real_T HighPassFilter_C2_m; + real_T LowPassFilter_C2_c; + real_T HighPassFilter_C2_h; + real_T LowPassFilter_C2_po; + real_T HighPassFilter_C2_i; + real_T LowPassFilter_C2_i; + real_T LeadLagFilter_C2; + real_T LeadLagFilter_C2_p; + real_T HighPassFilter_C2_e; + real_T LowPassFilter_C2_e; + real_T HighPassFilter_C3; + real_T LowPassFilter_C3; + real_T HighPassFilter_C3_f; + real_T LowPassFilter_C3_o; + real_T HighPassFilter_C3_n; + real_T LowPassFilter_C3_a; + real_T HighPassFilter_C3_j; + real_T LowPassFilter_C3_i; + real_T HighPassFilter_C3_k; + real_T LowPassFilter_C3_g; + real_T HighPassFilter_C3_m; + real_T LowPassFilter_C3_f; + real_T HighPassFilter_C3_d; + real_T LowPassFilter_C3_o5; + real_T LeadLagFilter_C3; + real_T LeadLagFilter_C3_m; + real_T HighPassFilter_C3_di; + real_T LowPassFilter_C3_l; + real_T HighPassFilter_C4; + real_T LowPassFilter_C4; + real_T HighPassFilter_C4_c; + real_T LowPassFilter_C4_o; + real_T HighPassFilter_C4_b; + real_T LowPassFilter_C4_b; + real_T HighPassFilter_C4_i; + real_T LowPassFilter_C4_k; + real_T HighPassFilter_C4_h; + real_T LowPassFilter_C4_d; + real_T HighPassFilter_C4_n; + real_T LowPassFilter_C4_dt; + real_T HighPassFilter_C4_nr; + real_T LowPassFilter_C4_f; + real_T LeadLagFilter_C4; + real_T LeadLagFilter_C4_k; + real_T HighPassFilter_C4_a; + real_T LowPassFilter_C4_a; + real_T DiscreteDerivativeVariableTs_Gain; + real_T DiscreteTimeIntegratorVariableTs_Gain; + real_T VS_Gain; + real_T VS_Gain_h; + real_T DiscreteDerivativeVariableTs1_Gain; + real_T DiscreteDerivativeVariableTs_Gain_g; + real_T VS_Gain_n; + real_T VS_Gain_j; + real_T VS_Gain_e; + real_T VS_Gain_n5; + real_T VS_Gain_nx; + real_T VS_Gain_ne; + real_T RateLimiterVariableTs_InitialCondition; + real_T DiscreteDerivativeVariableTs_InitialCondition; + real_T DiscreteTimeIntegratorVariableTs_InitialCondition; + real_T RateLimiterVariableTs_InitialCondition_l; + real_T RateLimiterVariableTs_InitialCondition_i; + real_T RateLimiterVariableTs_InitialCondition_il; + real_T DiscreteDerivativeVariableTs1_InitialCondition; + real_T RateLimiterVariableTs_InitialCondition_m; + real_T DiscreteDerivativeVariableTs_InitialCondition_f; + real_T RateLimiterVariableTs1_InitialCondition; + real_T RateLimiterVariableTs_InitialCondition_p; + real_T DiscreteTimeIntegratorVariableTs_LowerLimit; + real_T ScheduledGain2_Table[7]; + real_T ScheduledGain_Table[6]; + real_T ScheduledGain_Table_p[3]; + real_T ScheduledGain_Table_o[7]; + real_T ScheduledGain_Table_e[7]; + real_T ScheduledGain_Table_j[8]; + real_T ScheduledGain3_Table[5]; + real_T ScheduledGain2_Table_p[8]; + real_T ScheduledGain1_Table[7]; + real_T DiscreteTimeIntegratorVariableTs_UpperLimit; + real_T Subsystem_Value; + real_T Subsystem_Value_n; + real_T CompareToConstant2_const; + real_T CompareToConstant1_const; + real_T CompareToConstant_const; + real_T CompareToConstant_const_k; + real_T CompareToConstant5_const; + real_T CompareToConstant4_const; + real_T CompareToConstant5_const_e; + real_T CompareToConstant1_const_c; + real_T CompareToConstant6_const; + real_T CompareToConstant5_const_a; + real_T CompareToConstant2_const_d; + real_T CompareToConstant2_const_e; + real_T CompareToConstant_const_l; + real_T CompareToConstant4_const_o; + real_T CompareGSTRACK_const; + real_T CompareGSTRACK2_const; + real_T CompareToConstant_const_kt; + real_T CompareToConstant6_const_e; + real_T CompareToConstant7_const; + real_T CompareToConstant6_const_d; + real_T CompareToConstant5_const_h; + real_T CompareToConstant2_const_j; + real_T CompareToConstant8_const; + real_T CompareToConstant_const_h; + real_T CompareToConstant4_const_e; + real_T RateLimiterVariableTs_lo; + real_T RateLimiterVariableTs_lo_n; + real_T RateLimiterVariableTs_lo_k; + real_T RateLimiterVariableTs_lo_b; + real_T RateLimiterVariableTs_lo_c; + real_T RateLimiterVariableTs1_lo; + real_T RateLimiterVariableTs_lo_o; + real_T RateLimiterVariableTs_up; + real_T RateLimiterVariableTs_up_h; + real_T RateLimiterVariableTs_up_n; + real_T RateLimiterVariableTs_up_b; + real_T RateLimiterVariableTs_up_d; + real_T RateLimiterVariableTs1_up; + real_T RateLimiterVariableTs_up_i; + real_T DetectChange_vinit; + real_T DetectChange1_vinit; + boolean_T CompareToConstant_const_hx; + boolean_T CompareToConstant_const_e; + real_T Gain1_Gain; + real_T Gain_Gain; + real_T Gain1_Gain_p; + real_T Gain_Gain_a; + real_T Gain1_Gain_h; + real_T Gain_Gain_e; + real_T Gain1_Gain_k; + real_T Gain_Gain_ae; + real_T Gain2_Gain; + real_T Gain_Gain_c; + real_T Saturation_UpperSat; + real_T Saturation_LowerSat; + real_T k_beta_Phi_Gain; + real_T Gain1_Gain_g; + real_T Gain1_Gain_l; + real_T Gain_Gain_f; + real_T Gain_Gain_g; + real_T Constant3_Value; + real_T Constant3_Value_c; + real_T Constant3_Value_d; + real_T Gain1_Gain_b; + real_T Gain2_Gain_g; + real_T Gain4_Gain; + real_T Saturation1_UpperSat; + real_T Saturation1_LowerSat; + real_T Saturation_UpperSat_a; + real_T Saturation_LowerSat_a; + real_T Constant_Value; + real_T Constant3_Value_h; + real_T beta_Value; + real_T beta_Value_e; + real_T beta_Value_b; + real_T beta_Value_i; + real_T beta_Value_c; + real_T Saturation_UpperSat_e; + real_T Saturation_LowerSat_f; + real_T Gain7_Gain; + real_T Gain1_Gain_j; + real_T Gain_Gain_i; + real_T Gain1_Gain_lt; + real_T beta1_Value; + real_T beta1_Value_h; + real_T beta1_Value_l; + real_T beta1_Value_m; + real_T beta1_Value_d; + real_T beta1_Value_hy; + real_T Gain3_Gain; + real_T Constant1_Value; + real_T Constant_Value_d; + real_T Constant_Value_g; + real_T Gain6_Gain; + real_T Switch1_Threshold; + real_T Constant1_Value_g; + real_T Gain5_Gain; + real_T Constant_Value_p; + real_T Gain6_Gain_j; + real_T Switch1_Threshold_f; + real_T Constant1_Value_a; + real_T Gain5_Gain_l; + real_T Y_Y0; + real_T Gain1_Gain_bs; + real_T Saturation1_UpperSat_a; + real_T Saturation1_LowerSat_i; + real_T Constant1_Value_h; + real_T Constant_Value_p0; + real_T Gain6_Gain_n; + real_T Switch1_Threshold_d; + real_T Constant1_Value_m; + real_T Gain5_Gain_b; + real_T Constant_Value_i; + real_T Gain6_Gain_g; + real_T Switch1_Threshold_h; + real_T Constant1_Value_f; + real_T Gain5_Gain_a; + real_T Switch_Threshold; + real_T Gain1_Gain_d; + real_T Gain2_Gain_h; + real_T Gain1_Gain_i; + real_T Gain6_Gain_f; + real_T Switch1_Threshold_j; + real_T Constant_Value_f; + real_T Constant1_Value_c; + real_T Gain1_Gain_kg; + real_T Constant2_Value; + real_T Gain6_Gain_a; + real_T Gain5_Gain_k; + real_T Switch_Threshold_h; + real_T Constant_Value_o; + real_T Constant1_Value_g5; + real_T Gain1_Gain_m; + real_T Constant2_Value_m; + real_T Gain6_Gain_fa; + real_T Gain5_Gain_n; + real_T Switch_Threshold_hz; + real_T Switch_Threshold_k; + real_T Gain1_Gain_m4; + real_T Gain1_Gain_n; + real_T Constant_Value_m; + real_T Constant_Value_b; + real_T Gain6_Gain_ai; + real_T Switch1_Threshold_c; + real_T Constant1_Value_m5; + real_T Gain5_Gain_h; + real_T Constant_Value_ow; + real_T Gain6_Gain_c; + real_T Switch1_Threshold_b; + real_T Constant1_Value_mi; + real_T Gain5_Gain_g; + real_T GainTheta_Gain; + real_T GainTheta1_Gain; + real_T Gain_Gain_d; + real_T Gainqk_Gain; + real_T Gain_Gain_m; + real_T Gain_Gain_de; + real_T Gainpk_Gain; + real_T Gain3_Gain_a; + real_T Gain_Gain_n; + real_T Constant1_Value_b; + real_T Saturation_UpperSat_p; + real_T Saturation_LowerSat_g; + real_T Gain1_Gain_ll; + real_T Saturation1_UpperSat_j; + real_T Saturation1_LowerSat_d; + real_T Gain2_Gain_b; + real_T Constant3_Value_e; + real_T Constant3_Value_b; + real_T Constant3_Value_i; + real_T Constant3_Value_n; + real_T tau_Value; + real_T zeta_Value; + real_T Gain1_Gain_f; + real_T Saturation_UpperSat_b; + real_T Saturation_LowerSat_n; + real_T Gain_Gain_h; + real_T Constant3_Value_p; + real_T Constant3_Value_c2; + real_T Gain2_Gain_i; + real_T Gain3_Gain_i; + real_T Constant3_Value_if; + real_T Constant1_Value_fk; + real_T Constant3_Value_nr; + real_T Constant3_Value_hr; + real_T Gain1_Gain_nr; + real_T Saturation_UpperSat_o; + real_T Saturation_LowerSat_o; + real_T Gain2_Gain_gs; + real_T Saturation1_UpperSat_g; + real_T Saturation1_LowerSat_k; + real_T Gain6_Gain_b; + real_T Constant3_Value_n1; + real_T Constant3_Value_o; + real_T Constant3_Value_dk; + real_T Gain1_Gain_fq; + real_T Gain_Gain_o; + real_T Constant1_Value_e; + real_T Constant3_Value_m; + real_T Gain_Gain_fn; + real_T Constant2_Value_l; + real_T Constant3_Value_cd; + real_T Gain_Gain_cy; + real_T Gain1_Gain_o; + real_T Gain_Gain_oc; + real_T Constant3_Value_k; + real_T Gain_Gain_p; + real_T Gain1_Gain_i4; + real_T Gain_Gain_l; + real_T tau_Value_c; + real_T zeta_Value_h; + real_T Gain_Gain_lu; + real_T Gain5_Gain_o; + real_T Gain_Gain_b; + real_T Saturation_UpperSat_k; + real_T Saturation_LowerSat_f3; + real_T Constant_Value_a; + real_T Gain4_Gain_o; + real_T Switch_Threshold_n; + real_T Gain_Gain_m3; + real_T Saturation_UpperSat_c; + real_T Saturation_LowerSat_d; + real_T Gain1_Gain_kf; + real_T Saturation_UpperSat_m; + real_T Saturation_LowerSat_fw; + real_T Constant_Value_ii; + real_T Constant1_Value_i; + real_T Constant_Value_dh; + real_T Gain_Gain_ft; + real_T Saturation_UpperSat_n; + real_T Saturation_LowerSat_d4; + real_T ftmintoms_Gain; + real_T kntoms_Gain; + real_T Saturation_UpperSat_ar; + real_T Saturation_LowerSat_n5; + real_T Gain_Gain_k; + real_T ftmintoms_Gain_c; + real_T kntoms_Gain_h; + real_T Saturation_UpperSat_d; + real_T Saturation_LowerSat_nr; + real_T Gain_Gain_es; + real_T fpmtoms_Gain; + real_T kntoms_Gain_m; + real_T Saturation_UpperSat_j; + real_T Saturation_LowerSat_i; + real_T Gain_Gain_e3; + real_T Gain1_Gain_go; + real_T fpmtoms_Gain_g; + real_T kntoms_Gain_b; + real_T Saturation_UpperSat_ei; + real_T Saturation_LowerSat_dz; + real_T Gain_Gain_c1; + real_T Gain1_Gain_lx; + real_T Constant_Value_dy; + real_T Gain1_Gain_c; + real_T Gain1_Gain_e; + real_T Gain1_Gain_pf; + real_T Gain_Gain_am; + real_T Gain1_Gain_lp; + real_T g_Gain; + real_T ktstomps_Gain; + real_T GStoGS_CAS_Gain; + real_T _Gain; + real_T ktstomps_Gain_b; + real_T ug_Gain; + real_T Gain1_Gain_bf; + real_T Gain_Gain_b0; + real_T Constant4_Value; + real_T Constant3_Value_nq; + real_T Gain1_Gain_ik; + real_T Gain_Gain_aj; + real_T Switch_Threshold_l; + real_T Gain1_Gain_oz; + real_T Gain_Gain_m0; + real_T fpmtoms_Gain_a; + real_T kntoms_Gain_p; + real_T Saturation_UpperSat_h; + real_T Saturation_LowerSat_e; + real_T Gain_Gain_d4; + real_T Gain1_Gain_kd; + real_T fpmtoms_Gain_c; + real_T kntoms_Gain_l; + real_T Saturation_UpperSat_i; + real_T Saturation_LowerSat_h; + real_T Gain_Gain_bs; + real_T Gain1_Gain_o4; + real_T Constant_Value_c; + real_T Gain1_Gain_j0; + real_T Gain1_Gain_lxx; + real_T Gain1_Gain_bk; + real_T Gain_Gain_id; + real_T Gain1_Gain_dv; + real_T g_Gain_h; + real_T ktstomps_Gain_g; + real_T GStoGS_CAS_Gain_m; + real_T _Gain_h; + real_T ktstomps_Gain_i; + real_T ug_Gain_a; + real_T Gain1_Gain_hm; + real_T Gain_Gain_kj; + real_T Constant2_Value_c; + real_T Constant1_Value_b4; + real_T Gain1_Gain_mz; + real_T Gain_Gain_ie; + real_T Switch_Threshold_b; + real_T Gain1_Gain_f1; + real_T Gain_Gain_lr; + real_T Constant_Value_ig; + real_T fpmtoms_Gain_p; + real_T kntoms_Gain_f; + real_T Saturation_UpperSat_eik; + real_T Saturation_LowerSat_ad; + real_T Gain_Gain_e33; + real_T Gain1_Gain_ok; + real_T Gain1_Gain_dh; + real_T fpmtoms_Gain_h; + real_T kntoms_Gain_a; + real_T Saturation_UpperSat_f; + real_T Saturation_LowerSat_c; + real_T Gain_Gain_nq; + real_T Gain1_Gain_cv; + real_T Constant_Value_l; + real_T Gain1_Gain_jd; + real_T Gain1_Gain_ct; + real_T Gain1_Gain_id; + real_T Gain_Gain_ms; + real_T Gain1_Gain_ca; + real_T g_Gain_j; + real_T ktstomps_Gain_f; + real_T GStoGS_CAS_Gain_l; + real_T _Gain_m; + real_T ktstomps_Gain_j; + real_T ug_Gain_o; + real_T Gain1_Gain_hu; + real_T Gain_Gain_bn; + real_T Gain1_Gain_hz; + real_T Gain_Gain_d4y; + real_T Constant3_Value_ix; + real_T ftmintoms_Gain_d; + real_T kntoms_Gain_n; + real_T Saturation_UpperSat_ju; + real_T Saturation_LowerSat_gw; + real_T Gain_Gain_nz; + real_T ftmintoms_Gain_l; + real_T kntoms_Gain_au; + real_T Saturation_UpperSat_l; + real_T Saturation_LowerSat_hm; + real_T Gain_Gain_ey; + real_T fpmtoms_Gain_o; + real_T kntoms_Gain_o; + real_T Saturation_UpperSat_fr; + real_T Saturation_LowerSat_cd; + real_T Gain_Gain_lx; + real_T Gain1_Gain_jn; + real_T fpmtoms_Gain_e; + real_T kntoms_Gain_d; + real_T Saturation_UpperSat_hb; + real_T Saturation_LowerSat_k; + real_T Gain_Gain_in; + real_T Gain1_Gain_ps; + real_T Constant_Value_od; + real_T Gain1_Gain_hi; + real_T Gain1_Gain_da; + real_T Gain1_Gain_hg; + real_T Gain_Gain_b5; + real_T Gain1_Gain_kdq; + real_T g_Gain_m; + real_T ktstomps_Gain_m; + real_T GStoGS_CAS_Gain_k; + real_T _Gain_k; + real_T ktstomps_Gain_c; + real_T ug_Gain_aa; + real_T Gain1_Gain_gf; + real_T Gain_Gain_j; + real_T Constant4_Value_f; + real_T Constant3_Value_h1; + real_T Gain1_Gain_ov; + real_T Gain_Gain_jy; + real_T Switch_Threshold_o; + real_T Gain1_Gain_dvi; + real_T Gain_Gain_h4; + real_T fpmtoms_Gain_p3; + real_T kntoms_Gain_bq; + real_T Saturation_UpperSat_ba; + real_T Saturation_LowerSat_p; + real_T Gain_Gain_py; + real_T Gain1_Gain_hk; + real_T fpmtoms_Gain_j; + real_T kntoms_Gain_l5; + real_T Saturation_UpperSat_b3; + real_T Saturation_LowerSat_es; + real_T Gain_Gain_e5; + real_T Gain1_Gain_ja; + real_T Constant_Value_ia; + real_T Gain1_Gain_er; + real_T Gain1_Gain_fl; + real_T Gain1_Gain_ero; + real_T Gain_Gain_mx; + real_T Gain1_Gain_hv; + real_T g_Gain_g; + real_T ktstomps_Gain_a; + real_T GStoGS_CAS_Gain_n; + real_T _Gain_i; + real_T ktstomps_Gain_o; + real_T ug_Gain_f; + real_T Gain1_Gain_ot; + real_T Gain_Gain_dm; + real_T Constant2_Value_k; + real_T Constant1_Value_d; + real_T Gain1_Gain_ou; + real_T Gain_Gain_jg; + real_T Switch_Threshold_a; + real_T Gain1_Gain_gy; + real_T Gain_Gain_eq; + real_T Constant_Value_ga; + real_T fpmtoms_Gain_ps; + real_T kntoms_Gain_c; + real_T Saturation_UpperSat_oz; + real_T Saturation_LowerSat_ou; + real_T Gain_Gain_gt; + real_T Gain_Gain_c3; + real_T fpmtoms_Gain_d; + real_T kntoms_Gain_cv; + real_T Saturation_UpperSat_bb; + real_T Saturation_LowerSat_a4; + real_T Gain_Gain_hv; + real_T Gain1_Gain_ej; + real_T fpmtoms_Gain_f; + real_T kntoms_Gain_k; + real_T Saturation_UpperSat_pj; + real_T Saturation_LowerSat_py; + real_T Gain_Gain_bf; + real_T Gain1_Gain_jv; + real_T Constant_Value_lf; + real_T Gain1_Gain_gfa; + real_T Gain1_Gain_kw; + real_T Gain1_Gain_j4; + real_T Gain_Gain_bc; + real_T Gain1_Gain_n4; + real_T g_Gain_l; + real_T ktstomps_Gain_j4; + real_T GStoGS_CAS_Gain_o; + real_T _Gain_kb; + real_T ktstomps_Gain_k; + real_T ug_Gain_n; + real_T Gain1_Gain_b1; + real_T Gain_Gain_d0; + real_T Constant4_Value_o; + real_T Constant3_Value_nk; + real_T Gain1_Gain_on; + real_T Gain_Gain_hy; + real_T Switch_Threshold_d; + real_T Gain1_Gain_m1; + real_T Gain_Gain_fnw; + real_T fpmtoms_Gain_o2; + real_T kntoms_Gain_hi; + real_T Saturation_UpperSat_cv; + real_T Saturation_LowerSat_hd; + real_T Gain_Gain_pp; + real_T Gain1_Gain_iw; + real_T fpmtoms_Gain_hz; + real_T kntoms_Gain_i; + real_T Saturation_UpperSat_nu; + real_T Saturation_LowerSat_ae; + real_T Gain_Gain_ej; + real_T Gain1_Gain_lw; + real_T Constant_Value_fo; + real_T Gain1_Gain_ky; + real_T Gain1_Gain_nrn; + real_T Gain1_Gain_ip; + real_T Gain_Gain_d3; + real_T Gain1_Gain_mx; + real_T g_Gain_hq; + real_T ktstomps_Gain_l; + real_T GStoGS_CAS_Gain_e; + real_T _Gain_ip; + real_T ktstomps_Gain_mh; + real_T ug_Gain_e; + real_T Gain1_Gain_be; + real_T Gain_Gain_gx; + real_T Constant2_Value_h; + real_T Constant1_Value_o; + real_T Gain1_Gain_nj; + real_T Gain_Gain_aq; + real_T Switch_Threshold_g; + real_T Gain1_Gain_fle; + real_T Gain_Gain_ko; + real_T Constant_Value_fov; + real_T Gain2_Gain_n; + real_T Gain1_Gain_ox; + real_T kn2ms_Gain; + real_T Gain_Gain_os; + real_T Gain4_Gain_n; + real_T Saturation_UpperSat_ko; + real_T Saturation_LowerSat_ez; + real_T Gain2_Gain_k; + real_T Gain_Gain_gm; + real_T Saturation_UpperSat_j1; + real_T Saturation_LowerSat_nq; + real_T Constant_Value_lu; + real_T Gain3_Gain_c; + real_T Saturation_UpperSat_e0; + real_T Saturation_LowerSat_ph; + real_T fpmtoms_Gain_g4; + real_T kntoms_Gain_k4; + real_T Saturation_UpperSat_eb; + real_T Saturation_LowerSat_gk; + real_T Gain_Gain_ow; + real_T Gain2_Gain_l; + real_T Bias_Bias; + real_T Gain1_Gain_d4; + real_T Bias1_Bias; + real_T Gain_Gain_eyl; + real_T Constant2_Value_f; + real_T Gain4_Gain_oy; + real_T Gain5_Gain_c; + real_T kntofpm_Gain; + real_T maxslope_Gain; + real_T Gain1_Gain_oa; + real_T ftmintoms_Gain_k; + real_T kntoms_Gain_av; + real_T Saturation_UpperSat_i0; + real_T Saturation_LowerSat_nd; + real_T Gain_Gain_gr; + real_T Gain1_Gain_ml; + real_T ftmintoms_Gain_j; + real_T Saturation_UpperSat_ew; + real_T Saturation_LowerSat_an; + real_T Gain_Gain_by; + real_T Gain2_Gain_m; + real_T uDLookupTable_tableData[4]; + real_T uDLookupTable_bp01Data[4]; + real_T Constant1_Value_o0; + real_T Constant2_Value_kz; + real_T fpmtoms_Gain_po; + real_T kntoms_Gain_bh; + real_T Saturation_UpperSat_pd; + real_T Saturation_LowerSat_l; + real_T Gain_Gain_cr; + real_T Gain1_Gain_ga; + real_T Gain1_Gain_ol; + real_T fpmtoms_Gain_k; + real_T kntoms_Gain_py; + real_T Saturation_UpperSat_ec; + real_T Saturation_LowerSat_m; + real_T Gain_Gain_hc; + real_T Gain1_Gain_ln; + real_T Constant_Value_h; + real_T Gain1_Gain_hm2; + real_T Gain1_Gain_a; + real_T Gain1_Gain_it; + real_T Gain_Gain_er; + real_T Gain1_Gain_mxw; + real_T g_Gain_p; + real_T ktstomps_Gain_k5; + real_T GStoGS_CAS_Gain_j; + real_T _Gain_f; + real_T ktstomps_Gain_mf; + real_T ug_Gain_c; + real_T Gain1_Gain_nc; + real_T Gain_Gain_bg; + real_T Gain1_Gain_ke; + real_T Gain_Gain_c2; + real_T Constant3_Value_ew; + real_T ftmintoms_Gain_m; + real_T kntoms_Gain_om; + real_T Saturation_UpperSat_ed; + real_T Saturation_LowerSat_ee; + real_T Gain_Gain_kon; + real_T Constant_Value_iaf; + real_T ftmintoms_Gain_lv; + real_T kntoms_Gain_iw; + real_T Saturation_UpperSat_jt; + real_T Saturation_LowerSat_ih; + real_T Gain_Gain_o1; + real_T Gain2_Gain_hq; + real_T Saturation_UpperSat_f3; + real_T Saturation_LowerSat_b; + real_T ftmintoms_Gain_kr; + real_T kntoms_Gain_j; + real_T Saturation_UpperSat_nuy; + real_T Saturation_LowerSat_dj; + real_T Gain_Gain_fs; + real_T Gain_Gain_k2; + real_T Gain3_Gain_l; + real_T Saturation_UpperSat_ig; + real_T Saturation_LowerSat_ous; + real_T Gain_Gain_j0; + real_T Switch_Threshold_c; + real_T Gain_Gain_o2; + real_T Gain1_Gain_i0; + real_T Saturation_UpperSat_ix; + real_T Saturation_LowerSat_eq; + real_T Constant_Value_i4; + real_T Gain7_Gain_l; + boolean_T Delay_InitialCondition; + boolean_T Delay_InitialCondition_b; + uint8_T ManualSwitch_CurrentSetting; + uint8_T ManualSwitch_CurrentSetting_b; + }; + + AutopilotLawsModelClass(AutopilotLawsModelClass const&) = delete; + AutopilotLawsModelClass& operator= (AutopilotLawsModelClass const&) & = delete; + AutopilotLawsModelClass(AutopilotLawsModelClass &&) = delete; + AutopilotLawsModelClass& operator= (AutopilotLawsModelClass &&) = delete; + void setExternalInputs(const ExternalInputs_AutopilotLaws_T *pExternalInputs_AutopilotLaws_T) + { + AutopilotLaws_U = *pExternalInputs_AutopilotLaws_T; + } + + const ExternalOutputs_AutopilotLaws_T &getExternalOutputs() const + { + return AutopilotLaws_Y; + } + + void initialize(); + void step(); + static void terminate(); + AutopilotLawsModelClass(); + ~AutopilotLawsModelClass(); + private: + ExternalInputs_AutopilotLaws_T AutopilotLaws_U; + ExternalOutputs_AutopilotLaws_T AutopilotLaws_Y; + BlockIO_AutopilotLaws_T AutopilotLaws_B; + D_Work_AutopilotLaws_T AutopilotLaws_DWork; + static Parameters_AutopilotLaws_T AutopilotLaws_P; + static void AutopilotLaws_MATLABFunction(real_T rtu_tau, real_T rtu_zeta, real_T *rty_k2, real_T *rty_k1); + static void AutopilotLaws_LagFilter(real_T rtu_U, real_T rtu_C1, real_T rtu_dt, real_T *rty_Y, + rtDW_LagFilter_AutopilotLaws_T *localDW); + static void AutopilotLaws_RateLimiter(real_T rtu_u, real_T rtu_up, real_T rtu_lo, real_T rtu_Ts, real_T rtu_init, + real_T *rty_Y, rtDW_RateLimiter_AutopilotLaws_T *localDW); + static void AutopilotLaws_MATLABFunction_f_Init(rtDW_MATLABFunction_AutopilotLaws_d_T *localDW); + static void AutopilotLaws_MATLABFunction_m(real_T rtu_Psi_c, real_T rtu_dPsi, real_T rtu_Phi_c, real_T *rty_up, real_T + *rty_lo, rtDW_MATLABFunction_AutopilotLaws_d_T *localDW); + static void AutopilotLaws_Chart_Init(real_T *rty_out); + static void AutopilotLaws_Chart(real_T rtu_right, real_T rtu_left, boolean_T rtu_use_short_path, real_T *rty_out, + rtDW_Chart_AutopilotLaws_T *localDW); + static void AutopilotLaws_RateLimiter_n(boolean_T rtu_u, real_T rtu_up, real_T rtu_lo, real_T rtu_Ts, real_T rtu_init, + real_T *rty_Y, rtDW_RateLimiter_AutopilotLaws_l_T *localDW); + static void AutopilotLaws_Chart_g_Init(real_T *rty_out); + static void AutopilotLaws_Chart_h(real_T rtu_right, real_T rtu_left, real_T rtu_use_short_path, real_T *rty_out, + rtDW_Chart_AutopilotLaws_m_T *localDW); + static void AutopilotLaws_storevalue(boolean_T rtu_active, real_T rtu_u, real_T *rty_y, + rtDW_storevalue_AutopilotLaws_T *localDW); + static void AutopilotLaws_LeadLagFilter(real_T rtu_U, real_T rtu_C1, real_T rtu_C2, real_T rtu_C3, real_T rtu_C4, + real_T rtu_dt, real_T *rty_Y, rtDW_LeadLagFilter_AutopilotLaws_T *localDW); + static void AutopilotLaws_WashoutFilter(real_T rtu_U, real_T rtu_C1, real_T rtu_dt, real_T *rty_Y, + rtDW_WashoutFilter_AutopilotLaws_T *localDW); + static void AutopilotLaws_V_LSSpeedSelection1(real_T rtu_V_c, real_T rtu_VLS, real_T *rty_y); + static void AutopilotLaws_SpeedProtectionSignalSelection(const ap_laws_output *rtu_in, real_T rtu_VS_FD, real_T + rtu_VS_AP, real_T rtu_VLS_FD, real_T rtu_VLS_AP, real_T rtu_VMAX_FD, real_T rtu_VMAX_AP, real_T rtu_margin, real_T + *rty_FD, real_T *rty_AP); + static void AutopilotLaws_VSLimiter(real_T rtu_u, const ap_laws_output *rtu_in, real_T *rty_y); + static void AutopilotLaws_VSLimiter_f(real_T rtu_u, const ap_laws_output *rtu_in, real_T *rty_y); + static void AutopilotLaws_SignalEnablerGSTrack(real_T rtu_u, boolean_T rtu_e, real_T *rty_y); + static void AutopilotLaws_Voter1(real_T rtu_u1, real_T rtu_u2, real_T rtu_u3, real_T *rty_Y); +}; + +#endif + diff --git a/hdw-a339x/src/wasm/fbw_a320/src/model/AutopilotLaws_data.cpp b/hdw-a339x/src/wasm/fbw_a320/src/model/AutopilotLaws_data.cpp new file mode 100644 index 000000000..1e195dd7f --- /dev/null +++ b/hdw-a339x/src/wasm/fbw_a320/src/model/AutopilotLaws_data.cpp @@ -0,0 +1,1670 @@ +#include "AutopilotLaws.h" + +AutopilotLawsModelClass::Parameters_AutopilotLaws_T AutopilotLawsModelClass::AutopilotLaws_P{ + + { + { + 0.0, + 0.0 + }, + + { + { + 0.0, + 0.0, + 0.0 + }, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + false, + 0.0, + 0.0, + 0.0, + 0.0, + false, + 0.0, + 0.0, + + { + 0.0, + 0.0, + 0.0 + }, + false, + 0.0, + false, + 0.0, + + { + 0.0, + 0.0, + 0.0 + }, + false, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + false, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + false, + false, + false, + 0.0 + }, + + { + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + false, + false, + false, + false, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + false, + false, + false, + false, + false, + false, + false, + false + }, + + { + 0.0, + 0.0, + 0.0, + + { + 0.0, + 0.0, + 0.0 + }, + + { + 0.0, + 0.0, + 0.0 + }, + + { + false, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + } + } + }, + + + { 100.0, 150.0, 200.0, 250.0, 300.0, 400.0, 500.0 }, + + + { 0.0, 50.0, 100.0, 1000.0, 2500.0, 3000.0 }, + + + { 100.0, 150.0, 200.0, 250.0, 300.0, 400.0, 500.0 }, + + + { 100.0, 150.0, 200.0, 250.0, 300.0, 400.0, 500.0 }, + + + { 0.0, 150.0, 200.0 }, + + + { 0.0, 50.0, 100.0, 200.0, 400.0, 1000.0, 2500.0, 3000.0 }, + + + { 0.0, 100.0, 200.0, 400.0, 1000.0 }, + + + { 0.0, 50.0, 100.0, 200.0, 400.0, 1000.0, 2500.0, 3000.0 }, + + + { 0.0, 100.0, 150.0, 200.0, 250.0, 300.0, 400.0 }, + + 1.0, + + 1.0, + + 4.0, + + 2.0, + + 0.7, + + 2.0, + + 1.0, + + 1.0, + + 1.0, + + 2.0, + + 3.0, + + 0.33333333333333331, + + 2.0, + + 3.0, + + 0.33333333333333331, + + 2.0, + + 3.0, + + 0.33333333333333331, + + 2.0, + + 3.0, + + 0.33333333333333331, + + 2.0, + + 3.0, + + 0.33333333333333331, + + 2.0, + + 3.0, + + 0.33333333333333331, + + 2.0, + + 3.0, + + 0.33333333333333331, + + 10.0, + + 1.0, + + 4.0, + + 10.0, + + 10.0, + + 2.0, + + 1.0, + + 50.0, + + 0.033333333333333333, + + 15.0, + + 15.0, + + 2.0, + + 3.0, + + 0.33333333333333331, + + 1.0, + + 2.0, + + 6.0, + + 2.0, + + 4.0, + + 0.0, + + 0.0, + + 0.0, + + 0.0, + + 0.0, + + 0.0, + + 0.0, + + 0.0, + + 0.0, + + 0.0, + + 0.0, + + 0.0, + + 0.0, + + 0.0, + + 0.0, + + 0.0, + + 0.0, + + 0.0, + + 3.0, + + 1.0, + + 3.0, + + 1.0, + + 3.0, + + 1.0, + + 3.0, + + 1.0, + + 3.0, + + 1.0, + + 3.0, + + 1.0, + + 3.0, + + 1.0, + + 1.0, + + 1.0, + + 3.0, + + 1.0, + + 1.0, + + 0.33333333333333331, + + 1.0, + + 0.33333333333333331, + + 1.0, + + 0.33333333333333331, + + 1.0, + + 0.33333333333333331, + + 1.0, + + 0.33333333333333331, + + 1.0, + + 0.33333333333333331, + + 1.0, + + 0.33333333333333331, + + 15.0, + + 15.0, + + 1.0, + + 0.33333333333333331, + + 1.0, + + 1.0, + + 0.35, + + 0.35, + + 1.0, + + 1.0, + + 0.7, + + 1.0, + + 0.81, + + 1.0, + + 0.35, + + 0.7, + + 0.0, + + 0.0, + + 0.0, + + 0.0, + + 0.0, + + 0.0, + + 0.0, + + 0.0, + + 0.0, + + 0.0, + + 0.0, + + -3.0, + + + { 1.6, 1.6, 2.0, 2.8, 3.2, 4.2, 4.5 }, + + + { 3.7, 3.7, 4.3, 7.8, 15.0, 15.0 }, + + + { 1.6, 1.6, 2.0, 2.8, 3.2, 4.2, 4.5 }, + + + { 1.6, 1.6, 2.0, 2.8, 3.2, 4.2, 4.5 }, + + + { 0.8, 0.2, 0.2 }, + + + { 0.0, 0.0, -0.15, -0.4, -0.775, -1.6, -3.0, -3.0 }, + + + { 14.0, 14.0, 14.0, 14.0, 14.0 }, + + + { 0.0, 0.0, -0.1, -0.3, -0.6, -0.8, -2.0, -2.0 }, + + + { 2.5, 2.5, 2.0, 1.0, 1.0, 1.0, 1.0 }, + + 3.0, + + 1.0, + + 1.0, + + 4.0, + + 5.0, + + 30.0, + + 1.0, + + 2.0, + + 6.0, + + 1.0, + + 2.0, + + 0.0, + + 0.0, + + 0.5, + + 3.0, + + 0.0, + + 0.0, + + 31.0, + + 32.0, + + 60.0, + + 6.0, + + 7.0, + + 0.0, + + 0.0, + + 0.5, + + 8.0, + + 0.0, + + 0.0, + + -1000.0, + + -10.0, + + -1000.0, + + -10.0, + + -1000.0, + + -15.0, + + -10.0, + + 0.5, + + 10.0, + + 0.2, + + 1.0, + + 1.0, + + 15.0, + + 1.0, + + 0.0, + + 0.0, + + true, + + true, + + -1.0, + + 1.0, + + -1.0, + + 1.0, + + -1.0, + + 1.0, + + 1.0, + + -1.0, + + -1.0, + + -1.0, + + 45.0, + + -45.0, + + 0.4, + + -1.0, + + -1.0, + + 1.0, + + 1.0, + + 360.0, + + 360.0, + + 360.0, + + 0.017453292519943295, + + 0.2, + + 1.1, + + 15.0, + + -15.0, + + 1.0, + + 0.0, + + 1.0, + + 0.0, + + 0.0, + + 0.0, + + 0.0, + + 0.0, + + 0.0, + + 15.0, + + -15.0, + + 1.4, + + -1.0, + + 1.0, + + -1.0, + + 0.0, + + 0.0, + + 0.0, + + 0.0, + + 0.0, + + 0.0, + + 2.0, + + 0.0, + + 4.0, + + -4.0, + + 0.6, + + 0.0, + + 4.0, + + -0.93, + + -4.0, + + 0.6, + + 0.0, + + 4.0, + + -0.93, + + 0.0, + + -100.0, + + 400.0, + + -400.0, + + 0.0, + + -4.0, + + 0.6, + + 0.0, + + 4.0, + + -0.93, + + -4.0, + + 0.6, + + 0.0, + + 4.0, + + -0.93, + + 0.0, + + 2.85, + + 2.5, + + -50.0, + + 2.5, + + 0.0, + + 0.0, + + 4.0, + + -0.93, + + -4.0, + + 0.6, + + -0.93, + + 0.0, + + 0.0, + + 4.0, + + -0.93, + + -4.0, + + 0.6, + + -0.93, + + 0.0, + + 0.0, + + -1.0, + + -1.0, + + 0.0, + + -4.0, + + 0.6, + + 0.0, + + 4.0, + + -0.93, + + -4.0, + + 0.6, + + 0.0, + + 4.0, + + -0.93, + + -1.0, + + -1.0, + + 57.295779513082323, + + -1.0, + + 57.295779513082323, + + 57.295779513082323, + + -1.0, + + -1.0, + + 2.0, + + 1.0, + + 1.0, + + 0.0, + + 2.0, + + 1.0, + + 0.0, + + -25.0, + + 360.0, + + 360.0, + + 360.0, + + 360.0, + + 2.0, + + 0.8, + + 0.017453292519943295, + + 100.0, + + 2.0, + + 1.0, + + 360.0, + + 360.0, + + 1.0, + + 10.0, + + 360.0, + + 0.0, + + 360.0, + + -1.0, + + 1.16, + + -2.0, + + 360.0, + + -1.0, + + 1.16, + + -2.0, + + 3.0, + + 0.8, + + -1.0, + + 360.0, + + 360.0, + + 0.017453292519943295, + + 100.0, + + 2.0, + + 1200.0, + + 70.0, + + -70.0, + + 0.1, + + 360.0, + + 360.0, + + 360.0, + + 1.5, + + -1.0, + + 1.0, + + -1.2, + + 1.0, + + 1.0, + + 0.0, + + 1.0, + + 1.0, + + 0.0, + + -0.04, + + 1.0, + + -1.0, + + 360.0, + + -1.0, + + 1.0, + + -1.0, + + 1.0, + + 0.0, + + 1.0, + + 20.0, + + 0.0, + + 8.0, + + 1500.0, + + -1500.0, + + 0.00508, + + 0.51444444444444448, + + 2000.0, + + 1.0, + + 57.295779513082323, + + 0.00508, + + 0.51444444444444448, + + 2000.0, + + 1.0, + + 57.295779513082323, + + 0.00508, + + 0.51444444444444448, + + 1000.0, + + 1.0, + + 57.295779513082323, + + 0.017453292519943295, + + 0.00508, + + 0.51444444444444448, + + 1000.0, + + 1.0, + + 57.295779513082323, + + 0.017453292519943295, + + 1.0, + + 0.017453292519943295, + + 0.017453292519943295, + + 0.017453292519943295, + + 57.295779513082323, + + 0.017453292519943295, + + 9.81, + + 0.5144, + + 0.90350790290525129, + + 2.0, + + 0.5144, + + 0.1019367991845056, + + 0.017453292519943295, + + 57.295779513082323, + + 0.0, + + 1.0, + + 0.7, + + 57.295779513082323, + + 0.0, + + 0.33333333333333331, + + 0.36, + + 0.00508, + + 0.51444444444444448, + + 1000.0, + + 1.0, + + 57.295779513082323, + + 0.017453292519943295, + + 0.00508, + + 0.51444444444444448, + + 1000.0, + + 1.0, + + 57.295779513082323, + + 0.017453292519943295, + + 1.0, + + 0.017453292519943295, + + 0.017453292519943295, + + 0.017453292519943295, + + 57.295779513082323, + + 0.017453292519943295, + + 9.81, + + 0.5144, + + 0.90350790290525129, + + 2.0, + + 0.5144, + + 0.1019367991845056, + + 0.017453292519943295, + + 57.295779513082323, + + 0.0, + + 1.0, + + 0.7, + + 57.295779513082323, + + 0.0, + + 0.33333333333333331, + + 0.36, + + 5.0, + + 0.00508, + + 0.51444444444444448, + + 1000.0, + + 1.0, + + 57.295779513082323, + + 0.017453292519943295, + + 0.017453292519943295, + + 0.00508, + + 0.51444444444444448, + + 1000.0, + + 1.0, + + 57.295779513082323, + + 0.017453292519943295, + + 1.0, + + 0.017453292519943295, + + 0.017453292519943295, + + 0.017453292519943295, + + 57.295779513082323, + + 0.017453292519943295, + + 9.81, + + 0.5144, + + 0.90350790290525129, + + 2.0, + + 0.5144, + + 0.1019367991845056, + + 0.7, + + 57.295779513082323, + + 0.33333333333333331, + + 57.295779513082323, + + 120.0, + + 0.00508, + + 0.51444444444444448, + + 2000.0, + + 1.0, + + 57.295779513082323, + + 0.00508, + + 0.51444444444444448, + + 2000.0, + + 1.0, + + 57.295779513082323, + + 0.00508, + + 0.51444444444444448, + + 1000.0, + + 1.0, + + 57.295779513082323, + + 0.017453292519943295, + + 0.00508, + + 0.51444444444444448, + + 1000.0, + + 1.0, + + 57.295779513082323, + + 0.017453292519943295, + + 1.0, + + 0.017453292519943295, + + 0.017453292519943295, + + 0.017453292519943295, + + 57.295779513082323, + + 0.017453292519943295, + + 9.81, + + 0.5144, + + 0.90350790290525129, + + 2.0, + + 0.5144, + + 0.1019367991845056, + + 0.017453292519943295, + + 57.295779513082323, + + 0.0, + + 1.0, + + 0.7, + + 57.295779513082323, + + 0.0, + + 0.33333333333333331, + + 0.36, + + 0.00508, + + 0.51444444444444448, + + 1000.0, + + 1.0, + + 57.295779513082323, + + 0.017453292519943295, + + 0.00508, + + 0.51444444444444448, + + 1000.0, + + 1.0, + + 57.295779513082323, + + 0.017453292519943295, + + 1.0, + + 0.017453292519943295, + + 0.017453292519943295, + + 0.017453292519943295, + + 57.295779513082323, + + 0.017453292519943295, + + 9.81, + + 0.5144, + + 0.90350790290525129, + + 2.0, + + 0.5144, + + 0.1019367991845056, + + 0.017453292519943295, + + 57.295779513082323, + + 0.0, + + 1.0, + + 0.7, + + 57.295779513082323, + + 0.0, + + 0.33333333333333331, + + 0.36, + + 5.0, + + 0.00508, + + 0.51444444444444448, + + 1000.0, + + 1.0, + + 57.295779513082323, + + 0.35, + + 0.00508, + + 0.51444444444444448, + + 1000.0, + + 1.0, + + 57.295779513082323, + + 0.017453292519943295, + + 0.00508, + + 0.51444444444444448, + + 1000.0, + + 1.0, + + 57.295779513082323, + + 0.017453292519943295, + + 1.0, + + 0.017453292519943295, + + 0.017453292519943295, + + 0.017453292519943295, + + 57.295779513082323, + + 0.017453292519943295, + + 9.81, + + 0.5144, + + 0.90350790290525129, + + 2.0, + + 0.5144, + + 0.1019367991845056, + + 0.017453292519943295, + + 57.295779513082323, + + 0.0, + + 1.0, + + 0.7, + + 57.295779513082323, + + 0.0, + + 0.33333333333333331, + + 0.36, + + 0.00508, + + 0.51444444444444448, + + 1000.0, + + 1.0, + + 57.295779513082323, + + 0.017453292519943295, + + 0.00508, + + 0.51444444444444448, + + 1000.0, + + 1.0, + + 57.295779513082323, + + 0.017453292519943295, + + 1.0, + + 0.017453292519943295, + + 0.017453292519943295, + + 0.017453292519943295, + + 57.295779513082323, + + 0.017453292519943295, + + 9.81, + + 0.5144, + + 0.90350790290525129, + + 2.0, + + 0.5144, + + 0.1019367991845056, + + 0.017453292519943295, + + 57.295779513082323, + + 0.0, + + 1.0, + + 0.7, + + 57.295779513082323, + + 0.0, + + 0.33333333333333331, + + 0.36, + + 5.0, + + 0.016666666666666666, + + 0.017453292519943295, + + 0.51444444444444448, + + 3.2808398950131235, + + -0.25, + + 0.5, + + -0.5, + + 0.5, + + 1.0, + + 1.0, + + 0.0, + + 1.0, + + 1.0, + + -1.0, + + -4.0, + + 0.00508, + + 0.51444444444444448, + + 1000.0, + + 1.0, + + 57.295779513082323, + + 0.4, + + 2.0, + + 0.35, + + -2.0, + + 0.35, + + -2.0, + + -1.2, + + -0.3, + + 101.26859142607174, + + 0.02, + + 60.0, + + 0.00508, + + 0.51444444444444448, + + 2000.0, + + 1.0, + + 57.295779513082323, + + 0.3, + + 0.00508, + + 2000.0, + + 1.0, + + 57.295779513082323, + + 1.0, + + + { 1.0, 1.0, 1.15, 1.15 }, + + + { 0.0, 45000.0, 65000.0, 70000.0 }, + + 18.0, + + 50000.0, + + 0.00508, + + 0.51444444444444448, + + 1000.0, + + 1.0, + + 57.295779513082323, + + 0.017453292519943295, + + 0.017453292519943295, + + 0.00508, + + 0.51444444444444448, + + 1000.0, + + 1.0, + + 57.295779513082323, + + 0.017453292519943295, + + 1.0, + + 0.017453292519943295, + + 0.017453292519943295, + + 0.017453292519943295, + + 57.295779513082323, + + 0.017453292519943295, + + 9.81, + + 0.5144, + + 0.90350790290525129, + + 2.0, + + 0.5144, + + 0.1019367991845056, + + 0.7, + + 57.295779513082323, + + 0.33333333333333331, + + 57.295779513082323, + + 120.0, + + 0.00508, + + 0.51444444444444448, + + 2000.0, + + 1.0, + + 57.295779513082323, + + 120.0, + + 0.00508, + + 0.51444444444444448, + + 2000.0, + + 1.0, + + 57.295779513082323, + + 8.0, + + 1500.0, + + -1500.0, + + 0.00508, + + 0.51444444444444448, + + 2000.0, + + 1.0, + + 57.295779513082323, + + 0.25, + + -5.0, + + 2.0, + + -2.0, + + 1.3, + + 0.0, + + 0.25, + + -1.0, + + 1.0, + + 0.0, + + 1.0, + + 60.0, + + false, + + false, + + 0U, + + 0U +}; diff --git a/hdw-a339x/src/wasm/fbw_a320/src/model/AutopilotLaws_private.h b/hdw-a339x/src/wasm/fbw_a320/src/model/AutopilotLaws_private.h new file mode 100644 index 000000000..9ba1bca6a --- /dev/null +++ b/hdw-a339x/src/wasm/fbw_a320/src/model/AutopilotLaws_private.h @@ -0,0 +1,6 @@ +#ifndef RTW_HEADER_AutopilotLaws_private_h_ +#define RTW_HEADER_AutopilotLaws_private_h_ +#include "rtwtypes.h" +#include "AutopilotLaws_types.h" +#endif + diff --git a/hdw-a339x/src/wasm/fbw_a320/src/model/AutopilotLaws_types.h b/hdw-a339x/src/wasm/fbw_a320/src/model/AutopilotLaws_types.h new file mode 100644 index 000000000..fbe44093a --- /dev/null +++ b/hdw-a339x/src/wasm/fbw_a320/src/model/AutopilotLaws_types.h @@ -0,0 +1,344 @@ +#ifndef RTW_HEADER_AutopilotLaws_types_h_ +#define RTW_HEADER_AutopilotLaws_types_h_ +#include "rtwtypes.h" + +#ifndef DEFINED_TYPEDEF_FOR_ap_raw_laws_flare_ +#define DEFINED_TYPEDEF_FOR_ap_raw_laws_flare_ + +struct ap_raw_laws_flare +{ + boolean_T condition_Flare; + real_T H_dot_radio_fpm; + real_T H_dot_c_fpm; + real_T delta_Theta_H_dot_deg; + real_T delta_Theta_bz_deg; + real_T delta_Theta_bx_deg; + real_T delta_Theta_beta_c_deg; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_ap_raw_time_ +#define DEFINED_TYPEDEF_FOR_ap_raw_time_ + +struct ap_raw_time +{ + real_T dt; + real_T simulation_time; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_ap_lat_lon_alt_ +#define DEFINED_TYPEDEF_FOR_ap_lat_lon_alt_ + +struct ap_lat_lon_alt +{ + real_T lat; + real_T lon; + real_T alt; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_ap_raw_data_ +#define DEFINED_TYPEDEF_FOR_ap_raw_data_ + +struct ap_raw_data +{ + ap_lat_lon_alt aircraft_position; + real_T Theta_deg; + real_T Phi_deg; + real_T q_rad_s; + real_T r_rad_s; + real_T p_rad_s; + real_T V_ias_kn; + real_T V_tas_kn; + real_T V_mach; + real_T V_gnd_kn; + real_T alpha_deg; + real_T beta_deg; + real_T H_ft; + real_T H_ind_ft; + real_T H_radio_ft; + real_T H_dot_ft_min; + real_T Psi_magnetic_deg; + real_T Psi_magnetic_track_deg; + real_T Psi_true_deg; + real_T bx_m_s2; + real_T by_m_s2; + real_T bz_m_s2; + boolean_T nav_valid; + real_T nav_loc_deg; + real_T nav_gs_deg; + real_T nav_dme_valid; + real_T nav_dme_nmi; + boolean_T nav_loc_valid; + real_T nav_loc_magvar_deg; + real_T nav_loc_error_deg; + ap_lat_lon_alt nav_loc_position; + boolean_T nav_gs_valid; + real_T nav_gs_error_deg; + ap_lat_lon_alt nav_gs_position; + real_T flight_guidance_xtk_nmi; + real_T flight_guidance_tae_deg; + real_T flight_guidance_phi_deg; + real_T flight_guidance_phi_limit_deg; + real_T flight_phase; + real_T V2_kn; + real_T VAPP_kn; + real_T VLS_kn; + real_T VMAX_kn; + boolean_T is_flight_plan_available; + real_T altitude_constraint_ft; + real_T thrust_reduction_altitude; + real_T thrust_reduction_altitude_go_around; + real_T acceleration_altitude; + real_T acceleration_altitude_engine_out; + real_T acceleration_altitude_go_around; + real_T acceleration_altitude_go_around_engine_out; + real_T cruise_altitude; + real_T gear_strut_compression_1; + real_T gear_strut_compression_2; + real_T zeta_pos; + real_T throttle_lever_1_pos; + real_T throttle_lever_2_pos; + real_T flaps_handle_index; + boolean_T is_engine_operative_1; + boolean_T is_engine_operative_2; + real_T altimeter_setting_left_mbar; + real_T altimeter_setting_right_mbar; + real_T total_weight_kg; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_ap_raw_laws_input_ +#define DEFINED_TYPEDEF_FOR_ap_raw_laws_input_ + +struct ap_raw_laws_input +{ + real_T enabled_AP1; + real_T enabled_AP2; + real_T lateral_law; + real_T lateral_mode; + real_T lateral_mode_armed; + real_T vertical_law; + real_T vertical_mode; + real_T vertical_mode_armed; + real_T mode_reversion_lateral; + real_T mode_reversion_vertical; + real_T mode_reversion_vertical_target_fpm; + boolean_T mode_reversion_TRK_FPA; + boolean_T mode_reversion_triple_click; + boolean_T mode_reversion_fma; + boolean_T speed_protection_mode; + real_T autothrust_mode; + real_T Psi_c_deg; + real_T H_c_ft; + real_T H_dot_c_fpm; + real_T FPA_c_deg; + real_T V_c_kn; + boolean_T ALT_soft_mode_active; + boolean_T ALT_cruise_mode_active; + boolean_T EXPED_mode_active; + boolean_T FD_disconnect; + boolean_T FD_connect; + boolean_T TCAS_message_disarm; + boolean_T TCAS_message_RA_inhibit; + boolean_T TCAS_message_TRK_FPA_deselection; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_ap_laws_input_ +#define DEFINED_TYPEDEF_FOR_ap_laws_input_ + +struct ap_laws_input +{ + ap_raw_time time; + ap_raw_data data; + ap_raw_laws_input input; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_ap_data_ +#define DEFINED_TYPEDEF_FOR_ap_data_ + +struct ap_data +{ + ap_lat_lon_alt aircraft_position; + real_T Theta_deg; + real_T Phi_deg; + real_T qk_deg_s; + real_T rk_deg_s; + real_T pk_deg_s; + real_T V_ias_kn; + real_T V_tas_kn; + real_T V_mach; + real_T V_gnd_kn; + real_T alpha_deg; + real_T beta_deg; + real_T H_ft; + real_T H_ind_ft; + real_T H_radio_ft; + real_T H_dot_ft_min; + real_T Psi_magnetic_deg; + real_T Psi_magnetic_track_deg; + real_T Psi_true_deg; + real_T ax_m_s2; + real_T ay_m_s2; + real_T az_m_s2; + real_T bx_m_s2; + real_T by_m_s2; + real_T bz_m_s2; + boolean_T nav_valid; + real_T nav_loc_deg; + real_T nav_gs_deg; + real_T nav_dme_valid; + real_T nav_dme_nmi; + boolean_T nav_loc_valid; + real_T nav_loc_magvar_deg; + real_T nav_loc_error_deg; + ap_lat_lon_alt nav_loc_position; + boolean_T nav_e_loc_valid; + real_T nav_e_loc_error_deg; + boolean_T nav_gs_valid; + real_T nav_gs_error_deg; + ap_lat_lon_alt nav_gs_position; + boolean_T nav_e_gs_valid; + real_T nav_e_gs_error_deg; + real_T flight_guidance_xtk_nmi; + real_T flight_guidance_tae_deg; + real_T flight_guidance_phi_deg; + real_T flight_guidance_phi_limit_deg; + real_T flight_phase; + real_T V2_kn; + real_T VAPP_kn; + real_T VLS_kn; + real_T VMAX_kn; + boolean_T is_flight_plan_available; + real_T altitude_constraint_ft; + real_T thrust_reduction_altitude; + real_T thrust_reduction_altitude_go_around; + real_T acceleration_altitude; + real_T acceleration_altitude_engine_out; + real_T acceleration_altitude_go_around; + real_T acceleration_altitude_go_around_engine_out; + real_T cruise_altitude; + real_T on_ground; + real_T zeta_deg; + real_T throttle_lever_1_pos; + real_T throttle_lever_2_pos; + real_T flaps_handle_index; + boolean_T is_engine_operative_1; + boolean_T is_engine_operative_2; + boolean_T altimeter_setting_changed; + real_T total_weight_kg; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_ap_raw_output_command_ +#define DEFINED_TYPEDEF_FOR_ap_raw_output_command_ + +struct ap_raw_output_command +{ + real_T Theta_c_deg; + real_T Phi_c_deg; + real_T Beta_c_deg; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_ap_raw_output_ +#define DEFINED_TYPEDEF_FOR_ap_raw_output_ + +struct ap_raw_output +{ + real_T ap_on; + real_T Phi_loc_c; + real_T Nosewheel_c; + ap_raw_output_command flight_director; + ap_raw_output_command autopilot; + ap_raw_laws_flare flare_law; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_ap_laws_output_ +#define DEFINED_TYPEDEF_FOR_ap_laws_output_ + +struct ap_laws_output +{ + ap_raw_time time; + ap_data data; + ap_raw_laws_input input; + ap_raw_output output; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_ap_output_law_ +#define DEFINED_TYPEDEF_FOR_ap_output_law_ + +struct ap_output_law +{ + real_T flight_director; + real_T autopilot; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_vertical_mode_ +#define DEFINED_TYPEDEF_FOR_vertical_mode_ + +enum class vertical_mode + : int32_T { + NONE = 0, + ALT = 10, + ALT_CPT = 11, + OP_CLB = 12, + OP_DES = 13, + VS = 14, + FPA = 15, + ALT_CST = 20, + ALT_CST_CPT = 21, + CLB = 22, + DES = 23, + FINAL_DES = 24, + GS_CPT = 30, + GS_TRACK = 31, + LAND = 32, + FLARE = 33, + ROLL_OUT = 34, + SRS = 40, + SRS_GA = 41, + TCAS = 50 +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_lateral_mode_ +#define DEFINED_TYPEDEF_FOR_lateral_mode_ + +enum class lateral_mode + : int32_T { + NONE = 0, + HDG = 10, + TRACK = 11, + NAV = 20, + LOC_CPT = 30, + LOC_TRACK = 31, + LAND = 32, + FLARE = 33, + ROLL_OUT = 34, + RWY = 40, + RWY_TRACK = 41, + GA_TRACK = 50 +}; + +#endif +#endif + diff --git a/hdw-a339x/src/wasm/fbw_a320/src/model/ElacComputer.cpp b/hdw-a339x/src/wasm/fbw_a320/src/model/ElacComputer.cpp new file mode 100644 index 000000000..f8ba0130c --- /dev/null +++ b/hdw-a339x/src/wasm/fbw_a320/src/model/ElacComputer.cpp @@ -0,0 +1,1913 @@ +#include "ElacComputer.h" +#include "ElacComputer_types.h" +#include "rtwtypes.h" +#include +#include "look2_binlxpw.h" +#include "look1_binlxpw.h" +#include "LateralNormalLaw.h" +#include "LateralDirectLaw.h" +#include "PitchNormalLaw.h" +#include "PitchAlternateLaw.h" +#include "PitchDirectLaw.h" + +const uint8_T ElacComputer_IN_Flight{ 1U }; + +const uint8_T ElacComputer_IN_FlightToGroundTransition{ 2U }; + +const uint8_T ElacComputer_IN_Ground{ 3U }; + +const uint8_T ElacComputer_IN_NO_ACTIVE_CHILD{ 0U }; + +const uint8_T ElacComputer_IN_Flying{ 1U }; + +const uint8_T ElacComputer_IN_Landed{ 2U }; + +const uint8_T ElacComputer_IN_Landing100ft{ 3U }; + +const uint8_T ElacComputer_IN_Takeoff100ft{ 4U }; + +const real_T ElacComputer_RGND{ 0.0 }; + +void ElacComputer::ElacComputer_MATLABFunction(const base_arinc_429 *rtu_u, boolean_T *rty_y) +{ + *rty_y = (rtu_u->SSM != static_cast(SignStatusMatrix::FailureWarning)); +} + +void ElacComputer::ElacComputer_MATLABFunction_j(const base_arinc_429 *rtu_u, real_T rtu_bit, uint32_T *rty_y) +{ + real32_T tmp; + uint32_T a; + tmp = std::round(rtu_u->Data); + if (tmp < 4.2949673E+9F) { + if (tmp >= 0.0F) { + a = static_cast(tmp); + } else { + a = 0U; + } + } else { + a = MAX_uint32_T; + } + + if (-(rtu_bit - 1.0) >= 0.0) { + if (-(rtu_bit - 1.0) <= 31.0) { + a <<= static_cast(-(rtu_bit - 1.0)); + } else { + a = 0U; + } + } else if (-(rtu_bit - 1.0) >= -31.0) { + a >>= static_cast(rtu_bit - 1.0); + } else { + a = 0U; + } + + *rty_y = a & 1U; +} + +void ElacComputer::ElacComputer_RateLimiter_Reset(rtDW_RateLimiter_ElacComputer_T *localDW) +{ + localDW->pY_not_empty = false; +} + +void ElacComputer::ElacComputer_RateLimiter(real_T rtu_u, real_T rtu_up, real_T rtu_lo, real_T rtu_Ts, real_T rtu_init, + real_T *rty_Y, rtDW_RateLimiter_ElacComputer_T *localDW) +{ + if (!localDW->pY_not_empty) { + localDW->pY = rtu_init; + localDW->pY_not_empty = true; + } + + localDW->pY += std::fmax(std::fmin(rtu_u - localDW->pY, std::abs(rtu_up) * rtu_Ts), -std::abs(rtu_lo) * rtu_Ts); + *rty_Y = localDW->pY; +} + +void ElacComputer::ElacComputer_RateLimiter_o_Reset(rtDW_RateLimiter_ElacComputer_g_T *localDW) +{ + localDW->pY_not_empty = false; +} + +void ElacComputer::ElacComputer_RateLimiter_a(real_T rtu_u, real_T rtu_up, real_T rtu_lo, real_T rtu_Ts, real_T rtu_init, + boolean_T rtu_reset, real_T *rty_Y, rtDW_RateLimiter_ElacComputer_g_T *localDW) +{ + if ((!localDW->pY_not_empty) || rtu_reset) { + localDW->pY = rtu_init; + localDW->pY_not_empty = true; + } + + if (rtu_reset) { + *rty_Y = rtu_init; + } else { + *rty_Y = std::fmax(std::fmin(rtu_u - localDW->pY, std::abs(rtu_up) * rtu_Ts), -std::abs(rtu_lo) * rtu_Ts) + + localDW->pY; + } + + localDW->pY = *rty_Y; +} + +void ElacComputer::ElacComputer_MATLABFunction_o(boolean_T rtu_bit1, boolean_T rtu_bit2, boolean_T rtu_bit3, boolean_T + rtu_bit4, boolean_T rtu_bit5, boolean_T rtu_bit6, real_T *rty_handleIndex) +{ + if (rtu_bit1) { + *rty_handleIndex = 0.0; + } else if (rtu_bit2 && rtu_bit6) { + *rty_handleIndex = 1.0; + } else if (rtu_bit2 && (!rtu_bit6)) { + *rty_handleIndex = 2.0; + } else if (rtu_bit3) { + *rty_handleIndex = 3.0; + } else if (rtu_bit4) { + *rty_handleIndex = 4.0; + } else if (rtu_bit5) { + *rty_handleIndex = 5.0; + } else { + *rty_handleIndex = 0.0; + } +} + +void ElacComputer::ElacComputer_LagFilter_Reset(rtDW_LagFilter_ElacComputer_T *localDW) +{ + localDW->pY_not_empty = false; + localDW->pU_not_empty = false; +} + +void ElacComputer::ElacComputer_LagFilter(real_T rtu_U, real_T rtu_C1, real_T rtu_dt, real_T *rty_Y, + rtDW_LagFilter_ElacComputer_T *localDW) +{ + real_T ca; + real_T denom_tmp; + if ((!localDW->pY_not_empty) || (!localDW->pU_not_empty)) { + localDW->pU = rtu_U; + localDW->pU_not_empty = true; + localDW->pY = rtu_U; + localDW->pY_not_empty = true; + } + + denom_tmp = rtu_dt * rtu_C1; + ca = denom_tmp / (denom_tmp + 2.0); + *rty_Y = (2.0 - denom_tmp) / (denom_tmp + 2.0) * localDW->pY + (rtu_U * ca + localDW->pU * ca); + localDW->pY = *rty_Y; + localDW->pU = rtu_U; +} + +void ElacComputer::ElacComputer_MATLABFunction_g5_Reset(rtDW_MATLABFunction_ElacComputer_kz_T *localDW) +{ + localDW->output = false; + localDW->timeSinceCondition = 0.0; +} + +void ElacComputer::ElacComputer_MATLABFunction_c(boolean_T rtu_u, real_T rtu_Ts, boolean_T rtu_isRisingEdge, real_T + rtu_timeDelay, boolean_T *rty_y, rtDW_MATLABFunction_ElacComputer_kz_T *localDW) +{ + if (rtu_u == rtu_isRisingEdge) { + localDW->timeSinceCondition += rtu_Ts; + if (localDW->timeSinceCondition >= rtu_timeDelay) { + localDW->output = rtu_u; + } + } else { + localDW->timeSinceCondition = 0.0; + localDW->output = rtu_u; + } + + *rty_y = localDW->output; +} + +void ElacComputer::ElacComputer_MATLABFunction_k_Reset(rtDW_MATLABFunction_ElacComputer_o_T *localDW) +{ + localDW->output = false; +} + +void ElacComputer::ElacComputer_MATLABFunction_m(real_T rtu_u, real_T rtu_highTrigger, real_T rtu_lowTrigger, boolean_T * + rty_y, rtDW_MATLABFunction_ElacComputer_o_T *localDW) +{ + boolean_T output_tmp; + output_tmp = !localDW->output; + localDW->output = ((output_tmp && (rtu_u >= rtu_highTrigger)) || ((output_tmp || (rtu_u > rtu_lowTrigger)) && + localDW->output)); + *rty_y = localDW->output; +} + +void ElacComputer::ElacComputer_GetIASforMach4(real_T rtu_m, real_T rtu_m_t, real_T rtu_v, real_T *rty_v_t) +{ + *rty_v_t = rtu_v * rtu_m_t / rtu_m; +} + +void ElacComputer::ElacComputer_RateLimiter_d_Reset(rtDW_RateLimiter_ElacComputer_b_T *localDW) +{ + localDW->pY_not_empty = false; +} + +void ElacComputer::ElacComputer_RateLimiter_n(real_T rtu_u, real_T rtu_up, real_T rtu_lo, real_T rtu_Ts, boolean_T + rtu_reset, real_T *rty_Y, rtDW_RateLimiter_ElacComputer_b_T *localDW) +{ + if ((!localDW->pY_not_empty) || rtu_reset) { + localDW->pY = rtu_u; + localDW->pY_not_empty = true; + } + + if (rtu_reset) { + *rty_Y = rtu_u; + } else { + *rty_Y = std::fmax(std::fmin(rtu_u - localDW->pY, std::abs(rtu_up) * rtu_Ts), -std::abs(rtu_lo) * rtu_Ts) + + localDW->pY; + } + + localDW->pY = *rty_Y; +} + +void ElacComputer::ElacComputer_MATLABFunction_ax_Reset(rtDW_MATLABFunction_ElacComputer_b_T *localDW) +{ + localDW->previousInput_not_empty = false; +} + +void ElacComputer::ElacComputer_MATLABFunction_g(boolean_T rtu_u, boolean_T rtu_isRisingEdge, boolean_T *rty_y, + rtDW_MATLABFunction_ElacComputer_b_T *localDW) +{ + if (!localDW->previousInput_not_empty) { + localDW->previousInput = rtu_isRisingEdge; + localDW->previousInput_not_empty = true; + } + + if (rtu_isRisingEdge) { + *rty_y = (rtu_u && (!localDW->previousInput)); + } else { + *rty_y = ((!rtu_u) && localDW->previousInput); + } + + localDW->previousInput = rtu_u; +} + +void ElacComputer::ElacComputer_MATLABFunction_cw(const boolean_T rtu_u[19], real32_T *rty_y) +{ + uint32_T out; + out = 0U; + for (int32_T i{0}; i < 19; i++) { + out |= static_cast(rtu_u[i]) << (i + 10); + } + + *rty_y = static_cast(out); +} + +void ElacComputer::ElacComputer_LateralLawCaptoBits(lateral_efcs_law rtu_law, boolean_T *rty_bit1, boolean_T *rty_bit2) +{ + *rty_bit1 = (rtu_law == lateral_efcs_law::NormalLaw); + *rty_bit2 = (rtu_law == lateral_efcs_law::DirectLaw); +} + +void ElacComputer::step() +{ + real_T rtb_xi_deg; + real_T rtb_zeta_deg; + real_T rtb_eta_deg; + real_T rtb_eta_trim_dot_deg_s; + real_T rtb_eta_trim_limit_lo; + real_T rtb_eta_trim_limit_up; + real_T rtb_eta_deg_o; + real_T rtb_eta_trim_dot_deg_s_a; + real_T rtb_eta_trim_limit_lo_h; + real_T rtb_eta_trim_limit_up_d; + const base_arinc_429 *rtb_Switch1_g_0; + real_T abnormalCondition_tmp; + real_T rtb_BusAssignment_f_logic_ir_computation_data_n_z_g; + real_T rtb_BusAssignment_f_logic_ir_computation_data_theta_dot_deg_s; + real_T rtb_BusAssignment_p_logic_high_speed_prot_hi_thresh_kn; + real_T rtb_BusConversion_InsertedFor_BusAssignment_at_inport_8_BusCreator1_V_tas_kn; + real_T rtb_BusConversion_InsertedFor_BusAssignment_at_inport_8_BusCreator1_mach; + real_T rtb_DataTypeConversion3_m; + real_T rtb_DataTypeConversion5; + real_T rtb_DataTypeConversion6_g; + real_T rtb_DataTypeConversion8; + real_T rtb_DataTypeConversion_o; + real_T rtb_Switch3_p; + real_T rtb_Y; + real_T rtb_Y_d; + real_T rtb_Y_d_tmp_tmp; + real_T rtb_Y_n; + real_T rtb_Y_p; + real_T rtb_eta_trim_limit_lo_d; + real_T rtb_handleIndex; + real_T rtb_handleIndex_c; + real_T rtb_handleIndex_f; + real_T rtb_xi_deg_m; + real_T rtb_zeta_deg_f; + real_T u0; + real_T u0_0; + uint32_T rtb_DataTypeConversion1_j; + uint32_T rtb_Switch18; + uint32_T rtb_y_ee; + uint32_T rtb_y_g; + boolean_T rtb_VectorConcatenate[19]; + boolean_T rtb_VectorConcatenate_a[19]; + boolean_T rtb_NOT_k; + boolean_T rtb_OR1; + boolean_T rtb_OR1_hu; + boolean_T rtb_OR3; + boolean_T rtb_OR6; + boolean_T rtb_y_aq; + boolean_T rtb_y_k; + if (ElacComputer_U.in.sim_data.computer_running) { + real_T rtb_Sum2; + int32_T rtb_ap_special_disc; + real32_T rtb_V_ias; + real32_T rtb_V_tas; + real32_T rtb_alpha; + real32_T rtb_mach; + real32_T rtb_n_x; + real32_T rtb_n_y; + real32_T rtb_n_z; + real32_T rtb_phi; + real32_T rtb_phi_dot; + real32_T rtb_q; + real32_T rtb_r; + real32_T rtb_raComputationValue; + real32_T rtb_theta_dot; + real32_T rtb_tla1; + real32_T rtb_tla2; + boolean_T alternate1Condition_tmp; + boolean_T canEngageInRoll; + boolean_T hasPriorityInPitch; + boolean_T hasPriorityInRoll; + boolean_T leftAileronAvail; + boolean_T rightAileronAvail; + boolean_T rtb_AND1; + boolean_T rtb_AND1_h; + boolean_T rtb_AND2; + boolean_T rtb_AND3_b; + boolean_T rtb_AND4; + boolean_T rtb_BusAssignment_n_logic_is_yellow_hydraulic_power_avail; + boolean_T rtb_DataTypeConversion_m5; + boolean_T rtb_NOT_i; + boolean_T rtb_OR; + boolean_T rtb_OR1_me; + boolean_T rtb_OR4; + boolean_T rtb_OR7; + boolean_T rtb_OR_e1; + boolean_T rtb_aileronAntidroopActive; + boolean_T rtb_ap_authorised; + boolean_T rtb_doubleAdrFault; + boolean_T rtb_doubleIrFault; + boolean_T rtb_groundSettingHasPriority; + boolean_T rtb_isEngagedInPitch; + boolean_T rtb_isEngagedInRoll; + boolean_T rtb_leftAileronCrossCommandActive; + boolean_T rtb_rightAileronCrossCommandActive; + boolean_T rtb_rightElevatorAvail; + boolean_T rtb_thsAvail; + boolean_T rtb_thsAvail_tmp; + boolean_T rtb_tripleAdrFault; + boolean_T rtb_tripleIrFault; + lateral_efcs_law priorityPitchLateralLawCap; + lateral_efcs_law rtb_activeLateralLaw; + lateral_efcs_law rtb_lateralLawCapability; + lateral_efcs_law rtb_oppElacRollCapability; + pitch_efcs_law priorityPitchPitchLawCap; + pitch_efcs_law rtb_pitchLawCapability; + if (!ElacComputer_DWork.Runtime_MODE) { + ElacComputer_DWork.Delay_DSTATE_cc = ElacComputer_P.Delay_InitialCondition_c; + ElacComputer_DWork.Delay1_DSTATE = ElacComputer_P.Delay1_InitialCondition; + ElacComputer_DWork.Memory_PreviousInput = ElacComputer_P.SRFlipFlop_initial_condition; + ElacComputer_DWork.Delay_DSTATE = ElacComputer_P.DiscreteDerivativeVariableTs_InitialCondition; + ElacComputer_DWork.Delay_DSTATE_b = ElacComputer_P.Delay_InitialCondition; + ElacComputer_DWork.icLoad = true; + ElacComputer_LagFilter_Reset(&ElacComputer_DWork.sf_LagFilter_a); + ElacComputer_MATLABFunction_g5_Reset(&ElacComputer_DWork.sf_MATLABFunction_jz); + ElacComputer_MATLABFunction_g5_Reset(&ElacComputer_DWork.sf_MATLABFunction_lf); + ElacComputer_MATLABFunction_g5_Reset(&ElacComputer_DWork.sf_MATLABFunction_jl); + ElacComputer_DWork.ra1CoherenceRejected = false; + ElacComputer_DWork.ra2CoherenceRejected = false; + ElacComputer_DWork.configFullEventTime_not_empty = false; + ElacComputer_DWork.is_active_c30_ElacComputer = 0U; + ElacComputer_DWork.is_c30_ElacComputer = ElacComputer_IN_NO_ACTIVE_CHILD; + ElacComputer_DWork.on_ground_time = 0.0; + ElacComputer_B.in_flight = 0.0; + ElacComputer_MATLABFunction_k_Reset(&ElacComputer_DWork.sf_MATLABFunction_jg); + ElacComputer_MATLABFunction_g5_Reset(&ElacComputer_DWork.sf_MATLABFunction_cj); + ElacComputer_MATLABFunction_k_Reset(&ElacComputer_DWork.sf_MATLABFunction_mi); + ElacComputer_MATLABFunction_g5_Reset(&ElacComputer_DWork.sf_MATLABFunction_g2); + ElacComputer_MATLABFunction_k_Reset(&ElacComputer_DWork.sf_MATLABFunction_br); + ElacComputer_MATLABFunction_g5_Reset(&ElacComputer_DWork.sf_MATLABFunction_gfx); + ElacComputer_MATLABFunction_ax_Reset(&ElacComputer_DWork.sf_MATLABFunction_g4); + ElacComputer_MATLABFunction_ax_Reset(&ElacComputer_DWork.sf_MATLABFunction_nu); + ElacComputer_DWork.pLeftStickDisabled = false; + ElacComputer_DWork.pRightStickDisabled = false; + ElacComputer_MATLABFunction_g5_Reset(&ElacComputer_DWork.sf_MATLABFunction_j2); + ElacComputer_MATLABFunction_g5_Reset(&ElacComputer_DWork.sf_MATLABFunction_g24); + ElacComputer_MATLABFunction_g5_Reset(&ElacComputer_DWork.sf_MATLABFunction_nb); + ElacComputer_DWork.abnormalConditionWasActive = false; + ElacComputer_MATLABFunction_ax_Reset(&ElacComputer_DWork.sf_MATLABFunction_l0); + ElacComputer_RateLimiter_d_Reset(&ElacComputer_DWork.sf_RateLimiter_n); + ElacComputer_DWork.eventTime_not_empty_a = false; + ElacComputer_RateLimiter_d_Reset(&ElacComputer_DWork.sf_RateLimiter_m); + ElacComputer_DWork.is_active_c28_ElacComputer = 0U; + ElacComputer_DWork.is_c28_ElacComputer = ElacComputer_IN_NO_ACTIVE_CHILD; + ElacComputer_DWork.eventTime_not_empty = false; + ElacComputer_DWork.sProtActive = false; + ElacComputer_DWork.resetEventTime_not_empty = false; + ElacComputer_DWork.sProtActive_f = false; + ElacComputer_RateLimiter_Reset(&ElacComputer_DWork.sf_RateLimiter); + ElacComputer_RateLimiter_Reset(&ElacComputer_DWork.sf_RateLimiter_b); + LawMDLOBJ2.reset(); + LawMDLOBJ1.reset(); + ElacComputer_RateLimiter_o_Reset(&ElacComputer_DWork.sf_RateLimiter_a); + ElacComputer_RateLimiter_o_Reset(&ElacComputer_DWork.sf_RateLimiter_p); + ElacComputer_LagFilter_Reset(&ElacComputer_DWork.sf_LagFilter); + LawMDLOBJ5.reset(); + LawMDLOBJ3.reset(); + LawMDLOBJ4.reset(); + ElacComputer_MATLABFunction_g5_Reset(&ElacComputer_DWork.sf_MATLABFunction_fb); + ElacComputer_DWork.Runtime_MODE = true; + } + + rtb_OR1 = ((ElacComputer_U.in.bus_inputs.adr_1_bus.mach.SSM == static_cast(SignStatusMatrix:: + FailureWarning)) || (ElacComputer_U.in.bus_inputs.adr_1_bus.airspeed_computed_kn.SSM == static_cast + (SignStatusMatrix::FailureWarning)) || (ElacComputer_U.in.bus_inputs.adr_1_bus.airspeed_true_kn.SSM == + static_cast(SignStatusMatrix::FailureWarning)) || + (ElacComputer_U.in.bus_inputs.adr_1_bus.aoa_corrected_deg.SSM == static_cast(SignStatusMatrix:: + FailureWarning)) || ElacComputer_P.Constant1_Value_b || ElacComputer_P.Constant1_Value_b); + rtb_OR3 = ((ElacComputer_U.in.bus_inputs.adr_2_bus.mach.SSM == static_cast(SignStatusMatrix:: + FailureWarning)) || (ElacComputer_U.in.bus_inputs.adr_2_bus.airspeed_computed_kn.SSM == static_cast + (SignStatusMatrix::FailureWarning)) || (ElacComputer_U.in.bus_inputs.adr_2_bus.airspeed_true_kn.SSM == + static_cast(SignStatusMatrix::FailureWarning)) || + (ElacComputer_U.in.bus_inputs.adr_2_bus.aoa_corrected_deg.SSM == static_cast(SignStatusMatrix:: + FailureWarning)) || ElacComputer_P.Constant1_Value_b || ElacComputer_P.Constant1_Value_b); + rtb_OR4 = ((ElacComputer_U.in.bus_inputs.adr_3_bus.mach.SSM == static_cast(SignStatusMatrix:: + FailureWarning)) || (ElacComputer_U.in.bus_inputs.adr_3_bus.airspeed_computed_kn.SSM == static_cast + (SignStatusMatrix::FailureWarning)) || (ElacComputer_U.in.bus_inputs.adr_3_bus.airspeed_true_kn.SSM == + static_cast(SignStatusMatrix::FailureWarning)) || + (ElacComputer_U.in.bus_inputs.adr_3_bus.aoa_corrected_deg.SSM == static_cast(SignStatusMatrix:: + FailureWarning)) || ElacComputer_P.Constant1_Value_b || ElacComputer_P.Constant1_Value_b); + rtb_doubleAdrFault = ((rtb_OR1 && rtb_OR3) || (rtb_OR1 && rtb_OR4) || (rtb_OR3 && rtb_OR4)); + rtb_tripleAdrFault = (rtb_OR1 && rtb_OR3 && rtb_OR4); + rtb_OR = ((ElacComputer_U.in.bus_inputs.ir_1_bus.pitch_angle_deg.SSM != static_cast(SignStatusMatrix:: + NormalOperation)) || (ElacComputer_U.in.bus_inputs.ir_1_bus.roll_angle_deg.SSM != static_cast + (SignStatusMatrix::NormalOperation)) || (ElacComputer_U.in.bus_inputs.ir_1_bus.body_yaw_rate_deg_s.SSM != + static_cast(SignStatusMatrix::NormalOperation)) || + (ElacComputer_U.in.bus_inputs.ir_1_bus.body_long_accel_g.SSM != static_cast(SignStatusMatrix:: + NormalOperation)) || (ElacComputer_U.in.bus_inputs.ir_1_bus.body_lat_accel_g.SSM != static_cast + (SignStatusMatrix::NormalOperation)) || (ElacComputer_U.in.bus_inputs.ir_1_bus.body_normal_accel_g.SSM != + static_cast(SignStatusMatrix::NormalOperation)) || + (ElacComputer_U.in.bus_inputs.ir_1_bus.pitch_att_rate_deg_s.SSM != static_cast(SignStatusMatrix:: + NormalOperation)) || (ElacComputer_U.in.bus_inputs.ir_1_bus.roll_att_rate_deg_s.SSM != + static_cast(SignStatusMatrix::NormalOperation)) || ElacComputer_P.Constant_Value_ad); + rtb_OR6 = ((ElacComputer_U.in.bus_inputs.ir_2_bus.pitch_angle_deg.SSM != static_cast(SignStatusMatrix:: + NormalOperation)) || (ElacComputer_U.in.bus_inputs.ir_2_bus.roll_angle_deg.SSM != static_cast + (SignStatusMatrix::NormalOperation)) || (ElacComputer_U.in.bus_inputs.ir_2_bus.body_yaw_rate_deg_s.SSM + != static_cast(SignStatusMatrix::NormalOperation)) || + (ElacComputer_U.in.bus_inputs.ir_2_bus.body_long_accel_g.SSM != static_cast(SignStatusMatrix:: + NormalOperation)) || (ElacComputer_U.in.bus_inputs.ir_2_bus.body_lat_accel_g.SSM != static_cast + (SignStatusMatrix::NormalOperation)) || (ElacComputer_U.in.bus_inputs.ir_2_bus.body_normal_accel_g.SSM + != static_cast(SignStatusMatrix::NormalOperation)) || + (ElacComputer_U.in.bus_inputs.ir_2_bus.pitch_att_rate_deg_s.SSM != static_cast(SignStatusMatrix:: + NormalOperation)) || (ElacComputer_U.in.bus_inputs.ir_2_bus.roll_att_rate_deg_s.SSM != static_cast + (SignStatusMatrix::NormalOperation)) || ElacComputer_P.Constant_Value_ad); + rtb_OR7 = ((ElacComputer_U.in.bus_inputs.ir_3_bus.pitch_angle_deg.SSM != static_cast(SignStatusMatrix:: + NormalOperation)) || (ElacComputer_U.in.bus_inputs.ir_3_bus.roll_angle_deg.SSM != static_cast + (SignStatusMatrix::NormalOperation)) || (ElacComputer_U.in.bus_inputs.ir_3_bus.body_yaw_rate_deg_s.SSM + != static_cast(SignStatusMatrix::NormalOperation)) || + (ElacComputer_U.in.bus_inputs.ir_3_bus.body_long_accel_g.SSM != static_cast(SignStatusMatrix:: + NormalOperation)) || (ElacComputer_U.in.bus_inputs.ir_3_bus.body_lat_accel_g.SSM != static_cast + (SignStatusMatrix::NormalOperation)) || (ElacComputer_U.in.bus_inputs.ir_3_bus.body_normal_accel_g.SSM + != static_cast(SignStatusMatrix::NormalOperation)) || + (ElacComputer_U.in.bus_inputs.ir_3_bus.pitch_att_rate_deg_s.SSM != static_cast(SignStatusMatrix:: + NormalOperation)) || (ElacComputer_U.in.bus_inputs.ir_3_bus.roll_att_rate_deg_s.SSM != static_cast + (SignStatusMatrix::NormalOperation)) || ElacComputer_P.Constant_Value_ad); + rtb_tripleIrFault = (rtb_OR && rtb_OR6); + rtb_AND2 = (rtb_OR && rtb_OR7); + rtb_doubleIrFault = (rtb_tripleIrFault || rtb_AND2 || (rtb_OR6 && rtb_OR7)); + rtb_tripleIrFault = (rtb_tripleIrFault && rtb_OR7); + rtb_ap_authorised = !rtb_OR4; + rtb_AND1 = !rtb_OR3; + if (rtb_OR1 && rtb_AND1 && rtb_ap_authorised) { + rtb_V_ias = (ElacComputer_U.in.bus_inputs.adr_2_bus.airspeed_computed_kn.Data + + ElacComputer_U.in.bus_inputs.adr_3_bus.airspeed_computed_kn.Data) / 2.0F; + rtb_V_tas = (ElacComputer_U.in.bus_inputs.adr_2_bus.airspeed_true_kn.Data + + ElacComputer_U.in.bus_inputs.adr_3_bus.airspeed_true_kn.Data) / 2.0F; + rtb_mach = (ElacComputer_U.in.bus_inputs.adr_2_bus.mach.Data + ElacComputer_U.in.bus_inputs.adr_3_bus.mach.Data) / + 2.0F; + rtb_alpha = (ElacComputer_U.in.bus_inputs.adr_2_bus.aoa_corrected_deg.Data + + ElacComputer_U.in.bus_inputs.adr_3_bus.aoa_corrected_deg.Data) / 2.0F; + } else if ((!rtb_OR1) && rtb_OR3 && rtb_ap_authorised) { + rtb_V_ias = (ElacComputer_U.in.bus_inputs.adr_1_bus.airspeed_computed_kn.Data + + ElacComputer_U.in.bus_inputs.adr_3_bus.airspeed_computed_kn.Data) / 2.0F; + rtb_V_tas = (ElacComputer_U.in.bus_inputs.adr_1_bus.airspeed_true_kn.Data + + ElacComputer_U.in.bus_inputs.adr_3_bus.airspeed_true_kn.Data) / 2.0F; + rtb_mach = (ElacComputer_U.in.bus_inputs.adr_1_bus.mach.Data + ElacComputer_U.in.bus_inputs.adr_3_bus.mach.Data) / + 2.0F; + rtb_alpha = (ElacComputer_U.in.bus_inputs.adr_1_bus.aoa_corrected_deg.Data + + ElacComputer_U.in.bus_inputs.adr_3_bus.aoa_corrected_deg.Data) / 2.0F; + } else if (((!rtb_OR1) && rtb_AND1 && rtb_ap_authorised) || ((!rtb_OR1) && rtb_AND1 && rtb_OR4)) { + rtb_V_ias = (ElacComputer_U.in.bus_inputs.adr_1_bus.airspeed_computed_kn.Data + + ElacComputer_U.in.bus_inputs.adr_2_bus.airspeed_computed_kn.Data) / 2.0F; + rtb_V_tas = (ElacComputer_U.in.bus_inputs.adr_1_bus.airspeed_true_kn.Data + + ElacComputer_U.in.bus_inputs.adr_2_bus.airspeed_true_kn.Data) / 2.0F; + rtb_mach = (ElacComputer_U.in.bus_inputs.adr_1_bus.mach.Data + ElacComputer_U.in.bus_inputs.adr_2_bus.mach.Data) / + 2.0F; + rtb_alpha = (ElacComputer_U.in.bus_inputs.adr_1_bus.aoa_corrected_deg.Data + + ElacComputer_U.in.bus_inputs.adr_2_bus.aoa_corrected_deg.Data) / 2.0F; + } else if ((!rtb_OR1) && rtb_OR3 && rtb_OR4) { + rtb_V_ias = ElacComputer_U.in.bus_inputs.adr_1_bus.airspeed_computed_kn.Data; + rtb_V_tas = ElacComputer_U.in.bus_inputs.adr_1_bus.airspeed_true_kn.Data; + rtb_mach = ElacComputer_U.in.bus_inputs.adr_1_bus.mach.Data; + rtb_alpha = ElacComputer_U.in.bus_inputs.adr_1_bus.aoa_corrected_deg.Data; + } else if (rtb_OR1 && rtb_AND1 && rtb_OR4) { + rtb_V_ias = ElacComputer_U.in.bus_inputs.adr_2_bus.airspeed_computed_kn.Data; + rtb_V_tas = ElacComputer_U.in.bus_inputs.adr_2_bus.airspeed_true_kn.Data; + rtb_mach = ElacComputer_U.in.bus_inputs.adr_2_bus.mach.Data; + rtb_alpha = ElacComputer_U.in.bus_inputs.adr_2_bus.aoa_corrected_deg.Data; + } else if (rtb_OR1 && rtb_OR3 && rtb_ap_authorised) { + rtb_V_ias = ElacComputer_U.in.bus_inputs.adr_3_bus.airspeed_computed_kn.Data; + rtb_V_tas = ElacComputer_U.in.bus_inputs.adr_3_bus.airspeed_true_kn.Data; + rtb_mach = ElacComputer_U.in.bus_inputs.adr_3_bus.mach.Data; + rtb_alpha = ElacComputer_U.in.bus_inputs.adr_3_bus.aoa_corrected_deg.Data; + } else { + rtb_V_ias = 0.0F; + rtb_V_tas = 0.0F; + rtb_mach = 0.0F; + rtb_alpha = 0.0F; + } + + ElacComputer_LagFilter(static_cast(rtb_alpha), ElacComputer_P.LagFilter_C1, ElacComputer_U.in.time.dt, + &rtb_Y, &ElacComputer_DWork.sf_LagFilter_a); + rtb_eta_trim_limit_lo_d = rtb_V_ias; + rtb_BusConversion_InsertedFor_BusAssignment_at_inport_8_BusCreator1_V_tas_kn = rtb_V_tas; + rtb_BusConversion_InsertedFor_BusAssignment_at_inport_8_BusCreator1_mach = rtb_mach; + rtb_ap_authorised = !rtb_OR6; + rtb_AND1 = !rtb_OR7; + rtb_OR1 = (rtb_OR && rtb_AND1); + if (rtb_OR1 && rtb_ap_authorised) { + rtb_alpha = (ElacComputer_U.in.bus_inputs.ir_2_bus.pitch_angle_deg.Data + + ElacComputer_U.in.bus_inputs.ir_3_bus.pitch_angle_deg.Data) / 2.0F; + rtb_phi = (ElacComputer_U.in.bus_inputs.ir_2_bus.roll_angle_deg.Data + + ElacComputer_U.in.bus_inputs.ir_3_bus.roll_angle_deg.Data) / 2.0F; + rtb_q = (ElacComputer_U.in.bus_inputs.ir_2_bus.body_pitch_rate_deg_s.Data + + ElacComputer_U.in.bus_inputs.ir_3_bus.body_pitch_rate_deg_s.Data) / 2.0F; + rtb_r = (ElacComputer_U.in.bus_inputs.ir_2_bus.body_yaw_rate_deg_s.Data + + ElacComputer_U.in.bus_inputs.ir_3_bus.body_yaw_rate_deg_s.Data) / 2.0F; + rtb_n_x = (ElacComputer_U.in.bus_inputs.ir_2_bus.body_long_accel_g.Data + + ElacComputer_U.in.bus_inputs.ir_3_bus.body_long_accel_g.Data) / 2.0F; + rtb_n_y = (ElacComputer_U.in.bus_inputs.ir_2_bus.body_lat_accel_g.Data + + ElacComputer_U.in.bus_inputs.ir_3_bus.body_lat_accel_g.Data) / 2.0F; + rtb_n_z = (ElacComputer_U.in.bus_inputs.ir_2_bus.body_normal_accel_g.Data + + ElacComputer_U.in.bus_inputs.ir_3_bus.body_normal_accel_g.Data) / 2.0F; + rtb_theta_dot = (ElacComputer_U.in.bus_inputs.ir_2_bus.pitch_att_rate_deg_s.Data + + ElacComputer_U.in.bus_inputs.ir_3_bus.pitch_att_rate_deg_s.Data) / 2.0F; + rtb_phi_dot = (ElacComputer_U.in.bus_inputs.ir_2_bus.roll_att_rate_deg_s.Data + + ElacComputer_U.in.bus_inputs.ir_3_bus.roll_att_rate_deg_s.Data) / 2.0F; + } else { + rtb_OR = !rtb_OR; + rtb_OR7 = (rtb_OR && rtb_OR7); + if (rtb_OR7 && rtb_ap_authorised) { + rtb_alpha = (ElacComputer_U.in.bus_inputs.ir_1_bus.pitch_angle_deg.Data + + ElacComputer_U.in.bus_inputs.ir_2_bus.pitch_angle_deg.Data) / 2.0F; + rtb_phi = (ElacComputer_U.in.bus_inputs.ir_1_bus.roll_angle_deg.Data + + ElacComputer_U.in.bus_inputs.ir_2_bus.roll_angle_deg.Data) / 2.0F; + rtb_q = (ElacComputer_U.in.bus_inputs.ir_1_bus.body_pitch_rate_deg_s.Data + + ElacComputer_U.in.bus_inputs.ir_2_bus.body_pitch_rate_deg_s.Data) / 2.0F; + rtb_r = (ElacComputer_U.in.bus_inputs.ir_1_bus.body_yaw_rate_deg_s.Data + + ElacComputer_U.in.bus_inputs.ir_2_bus.body_yaw_rate_deg_s.Data) / 2.0F; + rtb_n_x = (ElacComputer_U.in.bus_inputs.ir_1_bus.body_long_accel_g.Data + + ElacComputer_U.in.bus_inputs.ir_2_bus.body_long_accel_g.Data) / 2.0F; + rtb_n_y = (ElacComputer_U.in.bus_inputs.ir_1_bus.body_lat_accel_g.Data + + ElacComputer_U.in.bus_inputs.ir_2_bus.body_lat_accel_g.Data) / 2.0F; + rtb_n_z = (ElacComputer_U.in.bus_inputs.ir_1_bus.body_normal_accel_g.Data + + ElacComputer_U.in.bus_inputs.ir_2_bus.body_normal_accel_g.Data) / 2.0F; + rtb_theta_dot = (ElacComputer_U.in.bus_inputs.ir_1_bus.pitch_att_rate_deg_s.Data + + ElacComputer_U.in.bus_inputs.ir_2_bus.pitch_att_rate_deg_s.Data) / 2.0F; + rtb_phi_dot = (ElacComputer_U.in.bus_inputs.ir_1_bus.roll_att_rate_deg_s.Data + + ElacComputer_U.in.bus_inputs.ir_2_bus.roll_att_rate_deg_s.Data) / 2.0F; + } else { + rtb_AND1 = (rtb_OR && rtb_AND1); + if ((rtb_AND1 && rtb_ap_authorised) || (rtb_AND1 && rtb_OR6)) { + rtb_alpha = (ElacComputer_U.in.bus_inputs.ir_1_bus.pitch_angle_deg.Data + + ElacComputer_U.in.bus_inputs.ir_3_bus.pitch_angle_deg.Data) / 2.0F; + rtb_phi = (ElacComputer_U.in.bus_inputs.ir_1_bus.roll_angle_deg.Data + + ElacComputer_U.in.bus_inputs.ir_3_bus.roll_angle_deg.Data) / 2.0F; + rtb_q = (ElacComputer_U.in.bus_inputs.ir_1_bus.body_pitch_rate_deg_s.Data + + ElacComputer_U.in.bus_inputs.ir_3_bus.body_pitch_rate_deg_s.Data) / 2.0F; + rtb_r = (ElacComputer_U.in.bus_inputs.ir_1_bus.body_yaw_rate_deg_s.Data + + ElacComputer_U.in.bus_inputs.ir_3_bus.body_yaw_rate_deg_s.Data) / 2.0F; + rtb_n_x = (ElacComputer_U.in.bus_inputs.ir_1_bus.body_long_accel_g.Data + + ElacComputer_U.in.bus_inputs.ir_3_bus.body_long_accel_g.Data) / 2.0F; + rtb_n_y = (ElacComputer_U.in.bus_inputs.ir_1_bus.body_lat_accel_g.Data + + ElacComputer_U.in.bus_inputs.ir_3_bus.body_lat_accel_g.Data) / 2.0F; + rtb_n_z = (ElacComputer_U.in.bus_inputs.ir_1_bus.body_normal_accel_g.Data + + ElacComputer_U.in.bus_inputs.ir_3_bus.body_normal_accel_g.Data) / 2.0F; + rtb_theta_dot = (ElacComputer_U.in.bus_inputs.ir_1_bus.pitch_att_rate_deg_s.Data + + ElacComputer_U.in.bus_inputs.ir_3_bus.pitch_att_rate_deg_s.Data) / 2.0F; + rtb_phi_dot = (ElacComputer_U.in.bus_inputs.ir_1_bus.roll_att_rate_deg_s.Data + + ElacComputer_U.in.bus_inputs.ir_3_bus.roll_att_rate_deg_s.Data) / 2.0F; + } else if (rtb_OR7 && rtb_OR6) { + rtb_alpha = ElacComputer_U.in.bus_inputs.ir_1_bus.pitch_angle_deg.Data; + rtb_phi = ElacComputer_U.in.bus_inputs.ir_1_bus.roll_angle_deg.Data; + rtb_q = ElacComputer_U.in.bus_inputs.ir_1_bus.body_pitch_rate_deg_s.Data; + rtb_r = ElacComputer_U.in.bus_inputs.ir_1_bus.body_yaw_rate_deg_s.Data; + rtb_n_x = ElacComputer_U.in.bus_inputs.ir_1_bus.body_long_accel_g.Data; + rtb_n_y = ElacComputer_U.in.bus_inputs.ir_1_bus.body_lat_accel_g.Data; + rtb_n_z = ElacComputer_U.in.bus_inputs.ir_1_bus.body_normal_accel_g.Data; + rtb_theta_dot = ElacComputer_U.in.bus_inputs.ir_1_bus.pitch_att_rate_deg_s.Data; + rtb_phi_dot = ElacComputer_U.in.bus_inputs.ir_1_bus.roll_att_rate_deg_s.Data; + } else if (rtb_OR1 && rtb_OR6) { + rtb_alpha = ElacComputer_U.in.bus_inputs.ir_3_bus.pitch_angle_deg.Data; + rtb_phi = ElacComputer_U.in.bus_inputs.ir_3_bus.roll_angle_deg.Data; + rtb_q = ElacComputer_U.in.bus_inputs.ir_3_bus.body_pitch_rate_deg_s.Data; + rtb_r = ElacComputer_U.in.bus_inputs.ir_3_bus.body_yaw_rate_deg_s.Data; + rtb_n_x = ElacComputer_U.in.bus_inputs.ir_3_bus.body_long_accel_g.Data; + rtb_n_y = ElacComputer_U.in.bus_inputs.ir_3_bus.body_lat_accel_g.Data; + rtb_n_z = ElacComputer_U.in.bus_inputs.ir_3_bus.body_normal_accel_g.Data; + rtb_theta_dot = ElacComputer_U.in.bus_inputs.ir_3_bus.pitch_att_rate_deg_s.Data; + rtb_phi_dot = ElacComputer_U.in.bus_inputs.ir_3_bus.roll_att_rate_deg_s.Data; + } else if (rtb_AND2 && rtb_ap_authorised) { + rtb_alpha = ElacComputer_U.in.bus_inputs.ir_2_bus.pitch_angle_deg.Data; + rtb_phi = ElacComputer_U.in.bus_inputs.ir_2_bus.roll_angle_deg.Data; + rtb_q = ElacComputer_U.in.bus_inputs.ir_2_bus.body_pitch_rate_deg_s.Data; + rtb_r = ElacComputer_U.in.bus_inputs.ir_2_bus.body_yaw_rate_deg_s.Data; + rtb_n_x = ElacComputer_U.in.bus_inputs.ir_2_bus.body_long_accel_g.Data; + rtb_n_y = ElacComputer_U.in.bus_inputs.ir_2_bus.body_lat_accel_g.Data; + rtb_n_z = ElacComputer_U.in.bus_inputs.ir_2_bus.body_normal_accel_g.Data; + rtb_theta_dot = ElacComputer_U.in.bus_inputs.ir_2_bus.pitch_att_rate_deg_s.Data; + rtb_phi_dot = ElacComputer_U.in.bus_inputs.ir_2_bus.roll_att_rate_deg_s.Data; + } else { + rtb_alpha = 0.0F; + rtb_phi = 0.0F; + rtb_q = 0.0F; + rtb_r = 0.0F; + rtb_n_x = 0.0F; + rtb_n_y = 0.0F; + rtb_n_z = 0.0F; + rtb_theta_dot = 0.0F; + rtb_phi_dot = 0.0F; + } + } + } + + rtb_Y_d_tmp_tmp = rtb_alpha; + rtb_Y_p = rtb_alpha; + rtb_DataTypeConversion5 = rtb_phi; + rtb_DataTypeConversion8 = rtb_r; + rtb_zeta_deg_f = rtb_r; + rtb_Y_n = rtb_r; + rtb_xi_deg_m = rtb_phi_dot; + ElacComputer_MATLABFunction_j(&ElacComputer_U.in.bus_inputs.sfcc_1_bus.slat_flap_actual_position_word, + ElacComputer_P.BitfromLabel_bit, &rtb_y_g); + ElacComputer_MATLABFunction(&ElacComputer_U.in.bus_inputs.sfcc_1_bus.slat_flap_actual_position_word, &rtb_y_k); + rtb_AND1 = ((rtb_y_g != 0U) && rtb_y_k); + ElacComputer_MATLABFunction_j(&ElacComputer_U.in.bus_inputs.sfcc_2_bus.slat_flap_actual_position_word, + ElacComputer_P.BitfromLabel1_bit, &rtb_y_g); + ElacComputer_MATLABFunction(&ElacComputer_U.in.bus_inputs.sfcc_2_bus.slat_flap_actual_position_word, &rtb_y_aq); + rtb_AND2 = ((rtb_y_g != 0U) && rtb_y_aq); + ElacComputer_MATLABFunction_c(std::abs(ElacComputer_U.in.bus_inputs.ra_1_bus.radio_height_ft.Data - + ElacComputer_U.in.bus_inputs.ra_2_bus.radio_height_ft.Data) > ElacComputer_P.CompareToConstant_const_ll, + ElacComputer_U.in.time.dt, ElacComputer_P.ConfirmNode_isRisingEdge, ElacComputer_P.ConfirmNode_timeDelay, + &rtb_NOT_k, &ElacComputer_DWork.sf_MATLABFunction_jz); + rtb_groundSettingHasPriority = (rtb_doubleAdrFault && ElacComputer_P.Constant1_Value_b); + rtb_OR6 = (rtb_tripleAdrFault || rtb_groundSettingHasPriority); + ElacComputer_MATLABFunction_c((ElacComputer_U.in.bus_inputs.ra_1_bus.radio_height_ft.Data > 50.0F) && + (ElacComputer_U.in.bus_inputs.ra_1_bus.radio_height_ft.SSM == static_cast(SignStatusMatrix:: + NormalOperation)) && (rtb_V_ias > 200.0F) && rtb_OR6, ElacComputer_U.in.time.dt, + ElacComputer_P.ConfirmNode2_isRisingEdge, ElacComputer_P.ConfirmNode2_timeDelay, &rtb_y_aq, + &ElacComputer_DWork.sf_MATLABFunction_lf); + ElacComputer_MATLABFunction_c((ElacComputer_U.in.bus_inputs.ra_2_bus.radio_height_ft.Data > 50.0F) && + (ElacComputer_U.in.bus_inputs.ra_2_bus.radio_height_ft.SSM == static_cast(SignStatusMatrix:: + NormalOperation)) && (rtb_V_ias > 200.0F) && rtb_OR6, ElacComputer_U.in.time.dt, + ElacComputer_P.ConfirmNode1_isRisingEdge, ElacComputer_P.ConfirmNode1_timeDelay, &rtb_y_k, + &ElacComputer_DWork.sf_MATLABFunction_jl); + ElacComputer_DWork.ra1CoherenceRejected = (rtb_y_aq || ElacComputer_DWork.ra1CoherenceRejected); + ElacComputer_DWork.ra2CoherenceRejected = (rtb_y_k || ElacComputer_DWork.ra2CoherenceRejected); + rtb_OR7 = ((ElacComputer_U.in.bus_inputs.ra_1_bus.radio_height_ft.SSM == static_cast(SignStatusMatrix:: + FailureWarning)) || ElacComputer_DWork.ra1CoherenceRejected); + rtb_OR = ((ElacComputer_U.in.bus_inputs.ra_2_bus.radio_height_ft.SSM == static_cast(SignStatusMatrix:: + FailureWarning)) || ElacComputer_DWork.ra2CoherenceRejected); + if (!ElacComputer_DWork.configFullEventTime_not_empty) { + ElacComputer_DWork.configFullEventTime = ElacComputer_U.in.time.simulation_time; + ElacComputer_DWork.configFullEventTime_not_empty = true; + } + + if ((!rtb_AND1) && (!rtb_AND2)) { + ElacComputer_DWork.configFullEventTime = ElacComputer_U.in.time.simulation_time; + } + + rtb_ap_authorised = !rtb_OR; + rtb_AND1 = !rtb_OR7; + if (rtb_AND1 && rtb_ap_authorised) { + if (rtb_NOT_k) { + if (ElacComputer_U.in.time.simulation_time > ElacComputer_DWork.configFullEventTime + 10.0) { + rtb_raComputationValue = std::fmin(ElacComputer_U.in.bus_inputs.ra_1_bus.radio_height_ft.Data, + ElacComputer_U.in.bus_inputs.ra_2_bus.radio_height_ft.Data); + } else { + rtb_raComputationValue = 250.0F; + } + } else { + rtb_raComputationValue = (ElacComputer_U.in.bus_inputs.ra_1_bus.radio_height_ft.Data + + ElacComputer_U.in.bus_inputs.ra_2_bus.radio_height_ft.Data) / 2.0F; + } + } else if ((rtb_OR7 && rtb_ap_authorised) || (rtb_AND1 && rtb_OR)) { + if ((rtb_V_ias > 180.0F) && rtb_OR6) { + rtb_raComputationValue = 250.0F; + } else if (rtb_OR) { + rtb_raComputationValue = ElacComputer_U.in.bus_inputs.ra_1_bus.radio_height_ft.Data; + } else { + rtb_raComputationValue = ElacComputer_U.in.bus_inputs.ra_2_bus.radio_height_ft.Data; + } + } else { + rtb_raComputationValue = 250.0F; + } + + rtb_AND1 = (rtb_OR7 && rtb_OR); + rtb_AND1_h = ((rtb_raComputationValue < ElacComputer_P.CompareToConstant_const) && (!rtb_AND1)); + rtb_OR6 = (ElacComputer_U.in.discrete_inputs.lgciu_1_left_main_gear_pressed && + ElacComputer_U.in.discrete_inputs.lgciu_1_right_main_gear_pressed); + rtb_aileronAntidroopActive = (ElacComputer_U.in.discrete_inputs.ground_spoilers_active_1 && + ElacComputer_U.in.discrete_inputs.ground_spoilers_active_2); + rtb_OR6 = ((rtb_OR6 && ElacComputer_U.in.discrete_inputs.lgciu_2_left_main_gear_pressed && + ElacComputer_U.in.discrete_inputs.lgciu_2_right_main_gear_pressed) || ((rtb_OR6 || + (ElacComputer_U.in.discrete_inputs.lgciu_2_left_main_gear_pressed && + ElacComputer_U.in.discrete_inputs.lgciu_2_right_main_gear_pressed)) && rtb_AND1_h) || (rtb_AND1_h && + rtb_aileronAntidroopActive)); + rtb_AND2 = (ElacComputer_U.in.sim_data.slew_on || ElacComputer_U.in.sim_data.pause_on || + ElacComputer_U.in.sim_data.tracking_mode_on_override); + if (ElacComputer_DWork.is_active_c30_ElacComputer == 0U) { + ElacComputer_DWork.is_active_c30_ElacComputer = 1U; + ElacComputer_DWork.is_c30_ElacComputer = ElacComputer_IN_Ground; + ElacComputer_B.in_flight = 0.0; + } else { + switch (ElacComputer_DWork.is_c30_ElacComputer) { + case ElacComputer_IN_Flight: + if (rtb_OR6 && (rtb_alpha < 2.5F)) { + ElacComputer_DWork.on_ground_time = ElacComputer_U.in.time.simulation_time; + ElacComputer_DWork.is_c30_ElacComputer = ElacComputer_IN_FlightToGroundTransition; + } else { + ElacComputer_B.in_flight = 1.0; + } + break; + + case ElacComputer_IN_FlightToGroundTransition: + if (ElacComputer_U.in.time.simulation_time - ElacComputer_DWork.on_ground_time >= 5.0) { + ElacComputer_DWork.is_c30_ElacComputer = ElacComputer_IN_Ground; + ElacComputer_B.in_flight = 0.0; + } else if ((!rtb_OR6) || (rtb_alpha >= 2.5F)) { + ElacComputer_DWork.on_ground_time = 0.0; + ElacComputer_DWork.is_c30_ElacComputer = ElacComputer_IN_Flight; + ElacComputer_B.in_flight = 1.0; + } + break; + + default: + if (((!rtb_OR6) && (rtb_alpha > 8.0F)) || (rtb_raComputationValue > 400.0F)) { + ElacComputer_DWork.on_ground_time = 0.0; + ElacComputer_DWork.is_c30_ElacComputer = ElacComputer_IN_Flight; + ElacComputer_B.in_flight = 1.0; + } else { + ElacComputer_B.in_flight = 0.0; + } + break; + } + } + + ElacComputer_MATLABFunction_m(ElacComputer_U.in.analog_inputs.yellow_hyd_pressure_psi, + ElacComputer_P.HysteresisNode2_highTrigger, ElacComputer_P.HysteresisNode2_lowTrigger, &rtb_y_aq, + &ElacComputer_DWork.sf_MATLABFunction_jg); + ElacComputer_MATLABFunction_c((!ElacComputer_U.in.discrete_inputs.yellow_low_pressure) && rtb_y_aq, + ElacComputer_U.in.time.dt, ElacComputer_P.ConfirmNode_isRisingEdge_k, ElacComputer_P.ConfirmNode_timeDelay_n, + &rtb_NOT_k, &ElacComputer_DWork.sf_MATLABFunction_cj); + ElacComputer_MATLABFunction_m(ElacComputer_U.in.analog_inputs.blue_hyd_pressure_psi, + ElacComputer_P.HysteresisNode1_highTrigger, ElacComputer_P.HysteresisNode1_lowTrigger, &rtb_y_aq, + &ElacComputer_DWork.sf_MATLABFunction_mi); + ElacComputer_MATLABFunction_c((!ElacComputer_U.in.discrete_inputs.blue_low_pressure) && rtb_y_aq, + ElacComputer_U.in.time.dt, ElacComputer_P.ConfirmNode1_isRisingEdge_i, ElacComputer_P.ConfirmNode1_timeDelay_h, + &rtb_y_k, &ElacComputer_DWork.sf_MATLABFunction_g2); + ElacComputer_MATLABFunction_m(ElacComputer_U.in.analog_inputs.green_hyd_pressure_psi, + ElacComputer_P.HysteresisNode3_highTrigger, ElacComputer_P.HysteresisNode3_lowTrigger, &rtb_y_aq, + &ElacComputer_DWork.sf_MATLABFunction_br); + ElacComputer_MATLABFunction_c((!ElacComputer_U.in.discrete_inputs.green_low_pressure) && rtb_y_aq, + ElacComputer_U.in.time.dt, ElacComputer_P.ConfirmNode2_isRisingEdge_j, ElacComputer_P.ConfirmNode2_timeDelay_k, + &rtb_y_aq, &ElacComputer_DWork.sf_MATLABFunction_gfx); + rtb_BusAssignment_f_logic_ir_computation_data_n_z_g = rtb_n_z; + rtb_BusAssignment_f_logic_ir_computation_data_theta_dot_deg_s = rtb_theta_dot; + rtb_handleIndex_f = rtb_raComputationValue; + rtb_OR7 = rtb_y_k; + rtb_OR = rtb_y_aq; + ElacComputer_MATLABFunction_g(ElacComputer_U.in.discrete_inputs.capt_priority_takeover_pressed, + ElacComputer_P.PulseNode_isRisingEdge, &rtb_y_k, &ElacComputer_DWork.sf_MATLABFunction_g4); + ElacComputer_MATLABFunction_g(ElacComputer_U.in.discrete_inputs.fo_priority_takeover_pressed, + ElacComputer_P.PulseNode1_isRisingEdge, &rtb_y_aq, &ElacComputer_DWork.sf_MATLABFunction_nu); + if (rtb_y_k) { + ElacComputer_DWork.pRightStickDisabled = true; + ElacComputer_DWork.pLeftStickDisabled = false; + } else if (rtb_y_aq) { + ElacComputer_DWork.pLeftStickDisabled = true; + ElacComputer_DWork.pRightStickDisabled = false; + } + + if (ElacComputer_DWork.pRightStickDisabled && ((!ElacComputer_U.in.discrete_inputs.capt_priority_takeover_pressed) && + (!ElacComputer_DWork.Delay1_DSTATE))) { + ElacComputer_DWork.pRightStickDisabled = false; + } else if (ElacComputer_DWork.pLeftStickDisabled) { + ElacComputer_DWork.pLeftStickDisabled = (ElacComputer_U.in.discrete_inputs.fo_priority_takeover_pressed || + ElacComputer_DWork.Delay_DSTATE_cc); + } + + ElacComputer_MATLABFunction_c(ElacComputer_DWork.pLeftStickDisabled && + (ElacComputer_U.in.discrete_inputs.fo_priority_takeover_pressed || ElacComputer_DWork.Delay_DSTATE_cc), + ElacComputer_U.in.time.dt, ElacComputer_P.ConfirmNode1_isRisingEdge_k, ElacComputer_P.ConfirmNode1_timeDelay_a, + &rtb_OR1, &ElacComputer_DWork.sf_MATLABFunction_j2); + ElacComputer_MATLABFunction_c(ElacComputer_DWork.pRightStickDisabled && + (ElacComputer_U.in.discrete_inputs.capt_priority_takeover_pressed || ElacComputer_DWork.Delay1_DSTATE), + ElacComputer_U.in.time.dt, ElacComputer_P.ConfirmNode_isRisingEdge_j, ElacComputer_P.ConfirmNode_timeDelay_a, + &rtb_OR3, &ElacComputer_DWork.sf_MATLABFunction_g24); + if (ElacComputer_DWork.pLeftStickDisabled) { + rtb_handleIndex_c = ElacComputer_P.Constant1_Value_p; + } else { + rtb_handleIndex_c = ElacComputer_U.in.analog_inputs.capt_roll_stick_pos; + } + + if (!ElacComputer_DWork.pRightStickDisabled) { + rtb_Sum2 = ElacComputer_U.in.analog_inputs.fo_roll_stick_pos; + } else { + rtb_Sum2 = ElacComputer_P.Constant1_Value_p; + } + + u0 = rtb_Sum2 + rtb_handleIndex_c; + if (u0 > ElacComputer_P.Saturation1_UpperSat) { + u0 = ElacComputer_P.Saturation1_UpperSat; + } else if (u0 < ElacComputer_P.Saturation1_LowerSat) { + u0 = ElacComputer_P.Saturation1_LowerSat; + } + + if (ElacComputer_U.in.discrete_inputs.is_unit_1) { + rtb_OR4 = ((!ElacComputer_U.in.discrete_inputs.l_elev_servo_failed) && rtb_OR7); + rtb_rightElevatorAvail = ((!ElacComputer_U.in.discrete_inputs.r_elev_servo_failed) && rtb_OR7); + } else { + rtb_OR4 = ((!ElacComputer_U.in.discrete_inputs.l_elev_servo_failed) && rtb_OR); + rtb_rightElevatorAvail = ((!ElacComputer_U.in.discrete_inputs.r_elev_servo_failed) && rtb_NOT_k); + } + + rtb_thsAvail_tmp = !ElacComputer_U.in.discrete_inputs.ths_motor_fault; + rtb_thsAvail = (rtb_thsAvail_tmp && (rtb_NOT_k || rtb_OR)); + if (ElacComputer_U.in.discrete_inputs.is_unit_1) { + rtb_ap_authorised = rtb_OR7; + } else { + rtb_ap_authorised = ((rtb_NOT_k && rtb_OR) || ((!rtb_OR7) && (rtb_OR || rtb_NOT_k))); + } + + rtb_thsAvail_tmp = ((!ElacComputer_U.in.discrete_inputs.r_elev_servo_failed) && + (!ElacComputer_U.in.discrete_inputs.l_elev_servo_failed) && rtb_thsAvail_tmp && + rtb_ap_authorised); + rtb_leftAileronCrossCommandActive = !ElacComputer_U.in.discrete_inputs.is_unit_1; + hasPriorityInPitch = (rtb_leftAileronCrossCommandActive || ElacComputer_U.in.discrete_inputs.opp_axis_pitch_failure); + rtb_isEngagedInPitch = (rtb_thsAvail_tmp && hasPriorityInPitch); + if (ElacComputer_U.in.discrete_inputs.is_unit_1) { + leftAileronAvail = ((!ElacComputer_U.in.discrete_inputs.l_ail_servo_failed) && rtb_OR7); + rightAileronAvail = ((!ElacComputer_U.in.discrete_inputs.r_ail_servo_failed) && rtb_OR); + } else { + leftAileronAvail = ((!ElacComputer_U.in.discrete_inputs.l_ail_servo_failed) && rtb_OR); + rightAileronAvail = ((!ElacComputer_U.in.discrete_inputs.r_ail_servo_failed) && rtb_OR7); + } + + canEngageInRoll = (leftAileronAvail || rightAileronAvail); + hasPriorityInRoll = (ElacComputer_U.in.discrete_inputs.is_unit_1 || + (ElacComputer_U.in.discrete_inputs.opp_left_aileron_lost && + ElacComputer_U.in.discrete_inputs.opp_right_aileron_lost)); + rtb_ap_authorised = !hasPriorityInRoll; + if (rtb_leftAileronCrossCommandActive && rtb_ap_authorised && + (ElacComputer_U.in.bus_inputs.elac_opp_bus.aileron_command_deg.SSM == static_cast(SignStatusMatrix:: + NormalOperation))) { + rtb_leftAileronCrossCommandActive = (ElacComputer_U.in.discrete_inputs.opp_left_aileron_lost && leftAileronAvail); + rtb_rightAileronCrossCommandActive = (ElacComputer_U.in.discrete_inputs.opp_right_aileron_lost && + rightAileronAvail); + } else { + rtb_leftAileronCrossCommandActive = false; + rtb_rightAileronCrossCommandActive = false; + } + + rtb_isEngagedInRoll = (canEngageInRoll && hasPriorityInRoll); + rtb_BusAssignment_n_logic_is_yellow_hydraulic_power_avail = rtb_NOT_k; + ElacComputer_MATLABFunction_j(&ElacComputer_U.in.bus_inputs.sec_1_bus.discrete_status_word_1, + ElacComputer_P.BitfromLabel_bit_c, &rtb_y_g); + rtb_AND3_b = (rtb_y_g != 0U); + ElacComputer_MATLABFunction_j(&ElacComputer_U.in.bus_inputs.sec_1_bus.discrete_status_word_1, + ElacComputer_P.BitfromLabel1_bit_j, &rtb_y_g); + rtb_AND1_h = (rtb_y_g != 0U); + ElacComputer_MATLABFunction_j(&ElacComputer_U.in.bus_inputs.sec_2_bus.discrete_status_word_1, + ElacComputer_P.BitfromLabel2_bit, &rtb_y_g); + rtb_AND4 = (rtb_y_g != 0U); + ElacComputer_MATLABFunction_j(&ElacComputer_U.in.bus_inputs.fcdc_1_bus.efcs_status_word_3, + ElacComputer_P.BitfromLabel3_bit, &rtb_y_g); + rtb_NOT_k = (rtb_y_g != 0U); + ElacComputer_MATLABFunction_j(&ElacComputer_U.in.bus_inputs.fcdc_2_bus.efcs_status_word_3, + ElacComputer_P.BitfromLabel5_bit, &rtb_y_g); + rtb_OR_e1 = (rtb_NOT_k || (rtb_y_g != 0U)); + ElacComputer_MATLABFunction_j(&ElacComputer_U.in.bus_inputs.fcdc_1_bus.efcs_status_word_3, + ElacComputer_P.BitfromLabel4_bit, &rtb_y_g); + rtb_NOT_k = (rtb_y_g != 0U); + ElacComputer_MATLABFunction_j(&ElacComputer_U.in.bus_inputs.fcdc_2_bus.efcs_status_word_3, + ElacComputer_P.BitfromLabel6_bit, &rtb_y_g); + rtb_OR1_me = (rtb_NOT_k || (rtb_y_g != 0U)); + ElacComputer_MATLABFunction_j(&ElacComputer_U.in.bus_inputs.fcdc_1_bus.efcs_status_word_1, + ElacComputer_P.BitfromLabel7_bit, &rtb_y_g); + rtb_NOT_k = (rtb_y_g == 0U); + ElacComputer_MATLABFunction_j(&ElacComputer_U.in.bus_inputs.fcdc_2_bus.efcs_status_word_1, + ElacComputer_P.BitfromLabel8_bit, &rtb_y_g); + if ((ElacComputer_U.in.discrete_inputs.fac_1_yaw_control_lost && + ElacComputer_U.in.discrete_inputs.fac_2_yaw_control_lost) || + ((ElacComputer_U.in.bus_inputs.sec_1_bus.discrete_status_word_1.SSM != static_cast(SignStatusMatrix:: + NormalOperation)) && (ElacComputer_U.in.bus_inputs.sec_2_bus.discrete_status_word_1.SSM != + static_cast(SignStatusMatrix::NormalOperation)) && ((!rtb_NOT_k) && (rtb_y_g != 0U))) || + ((!rtb_AND3_b) && (!rtb_AND1_h) && (!rtb_AND4) && (!rtb_OR_e1) && (!rtb_OR1_me))) { + rtb_lateralLawCapability = lateral_efcs_law::DirectLaw; + } else { + rtb_lateralLawCapability = lateral_efcs_law::NormalLaw; + } + + ElacComputer_MATLABFunction(&ElacComputer_U.in.bus_inputs.sec_1_bus.discrete_status_word_2, &rtb_y_k); + ElacComputer_MATLABFunction(&ElacComputer_U.in.bus_inputs.sec_2_bus.discrete_status_word_2, &rtb_y_aq); + ElacComputer_MATLABFunction_j(&ElacComputer_U.in.bus_inputs.sec_1_bus.discrete_status_word_2, + ElacComputer_P.BitfromLabel4_bit_d, &rtb_y_g); + rtb_NOT_k = (rtb_y_g != 0U); + ElacComputer_MATLABFunction_j(&ElacComputer_U.in.bus_inputs.sec_2_bus.discrete_status_word_2, + ElacComputer_P.BitfromLabel5_bit_e, &rtb_y_g); + rtb_OR1_hu = (((!rtb_y_k) && (!rtb_y_aq)) || (rtb_NOT_k && (rtb_y_g != 0U))); + ElacComputer_MATLABFunction_j(&ElacComputer_U.in.bus_inputs.sec_1_bus.discrete_status_word_2, + ElacComputer_P.BitfromLabel6_bit_k, &rtb_y_g); + rtb_NOT_k = (rtb_y_g != 0U); + ElacComputer_MATLABFunction_j(&ElacComputer_U.in.bus_inputs.sec_2_bus.discrete_status_word_2, + ElacComputer_P.BitfromLabel7_bit_h, &rtb_y_g); + rtb_NOT_k = (rtb_NOT_k || (rtb_y_g != 0U)); + ElacComputer_MATLABFunction_j(&ElacComputer_U.in.bus_inputs.sfcc_1_bus.slat_flap_actual_position_word, + ElacComputer_P.BitfromLabel_bit_a, &rtb_y_g); + ElacComputer_MATLABFunction(&ElacComputer_U.in.bus_inputs.sfcc_1_bus.slat_flap_actual_position_word, &rtb_y_k); + rtb_y_k = ((rtb_y_g != 0U) && rtb_y_k); + ElacComputer_MATLABFunction_j(&ElacComputer_U.in.bus_inputs.sfcc_2_bus.slat_flap_actual_position_word, + ElacComputer_P.BitfromLabel1_bit_jr, &rtb_y_g); + ElacComputer_MATLABFunction(&ElacComputer_U.in.bus_inputs.sfcc_2_bus.slat_flap_actual_position_word, &rtb_y_aq); + rtb_AND1_h = ((rtb_y_g != 0U) && rtb_y_aq); + ElacComputer_MATLABFunction_c(ElacComputer_U.in.sim_data.slew_on, ElacComputer_U.in.time.dt, + ElacComputer_P.ConfirmNode_isRisingEdge_o, ElacComputer_P.ConfirmNode_timeDelay_d, &rtb_y_aq, + &ElacComputer_DWork.sf_MATLABFunction_nb); + abnormalCondition_tmp = std::abs(static_cast(rtb_phi)); + rtb_NOT_i = !rtb_OR6; + rtb_AND3_b = !ElacComputer_P.Constant_Value_ad; + rtb_AND4 = ((!rtb_y_aq) && rtb_NOT_i && (((!rtb_tripleAdrFault) && ((rtb_mach > 0.91) || (rtb_Y < -10.0) || (rtb_Y > + 40.0) || (rtb_V_ias > 440.0F) || (rtb_V_ias < 60.0F))) || ((!rtb_tripleIrFault) && ((!rtb_doubleIrFault) || + rtb_AND3_b) && ((abnormalCondition_tmp > 125.0) || ((rtb_alpha > 50.0F) || (rtb_alpha < -30.0F)))))); + ElacComputer_DWork.abnormalConditionWasActive = (rtb_AND4 || (rtb_NOT_i && + ElacComputer_DWork.abnormalConditionWasActive)); + rtb_DataTypeConversion_m5 = ((!rtb_OR4) || (!rtb_rightElevatorAvail)); + rtb_OR_e1 = !leftAileronAvail; + rtb_OR1_me = !rightAileronAvail; + rtb_y_aq = (rtb_groundSettingHasPriority || rtb_tripleAdrFault || ElacComputer_DWork.abnormalConditionWasActive || + (rtb_OR_e1 && rtb_OR1_me && rtb_DataTypeConversion_m5)); + alternate1Condition_tmp = !ElacComputer_P.Constant1_Value_b; + rtb_groundSettingHasPriority = ((rtb_doubleIrFault && rtb_AND3_b) || (rtb_groundSettingHasPriority && + alternate1Condition_tmp) || (rtb_doubleAdrFault && alternate1Condition_tmp && alternate1Condition_tmp) || + rtb_DataTypeConversion_m5); + if (rtb_tripleIrFault || ((rtb_y_aq || rtb_groundSettingHasPriority || rtb_AND1 || (rtb_lateralLawCapability == + lateral_efcs_law::DirectLaw)) && ((ElacComputer_B.in_flight != 0.0) && ((rtb_NOT_k && (!rtb_OR1_hu)) || + ((rtb_y_k || rtb_AND1_h) && rtb_OR1_hu))))) { + rtb_pitchLawCapability = pitch_efcs_law::DirectLaw; + } else if (rtb_y_aq) { + rtb_pitchLawCapability = pitch_efcs_law::AlternateLaw2; + } else if (rtb_groundSettingHasPriority) { + rtb_pitchLawCapability = pitch_efcs_law::AlternateLaw1; + } else { + rtb_pitchLawCapability = pitch_efcs_law::NormalLaw; + } + + ElacComputer_MATLABFunction_j(&ElacComputer_U.in.bus_inputs.elac_opp_bus.discrete_status_word_2, + ElacComputer_P.BitfromLabel_bit_h, &rtb_y_ee); + ElacComputer_MATLABFunction_j(&ElacComputer_U.in.bus_inputs.elac_opp_bus.discrete_status_word_2, + ElacComputer_P.BitfromLabel1_bit_e, &rtb_Switch18); + ElacComputer_MATLABFunction_j(&ElacComputer_U.in.bus_inputs.elac_opp_bus.discrete_status_word_2, + ElacComputer_P.BitfromLabel2_bit_k, &rtb_DataTypeConversion1_j); + ElacComputer_MATLABFunction_j(&ElacComputer_U.in.bus_inputs.elac_opp_bus.discrete_status_word_2, + ElacComputer_P.BitfromLabel3_bit_m, &rtb_y_g); + if ((rtb_DataTypeConversion1_j != 0U) && (rtb_y_g == 0U)) { + rtb_oppElacRollCapability = lateral_efcs_law::NormalLaw; + } else if ((rtb_DataTypeConversion1_j == 0U) && (rtb_y_g != 0U)) { + rtb_oppElacRollCapability = lateral_efcs_law::DirectLaw; + } else { + rtb_oppElacRollCapability = lateral_efcs_law::None; + } + + if (hasPriorityInPitch && rtb_isEngagedInPitch) { + priorityPitchPitchLawCap = rtb_pitchLawCapability; + priorityPitchLateralLawCap = rtb_lateralLawCapability; + } else if ((!hasPriorityInPitch) || (!rtb_isEngagedInPitch)) { + if ((rtb_y_ee != 0U) && (rtb_Switch18 == 0U)) { + priorityPitchPitchLawCap = pitch_efcs_law::NormalLaw; + } else if ((rtb_y_ee == 0U) && (rtb_Switch18 != 0U)) { + priorityPitchPitchLawCap = pitch_efcs_law::AlternateLaw1; + } else if ((rtb_y_ee != 0U) && (rtb_Switch18 != 0U)) { + priorityPitchPitchLawCap = pitch_efcs_law::DirectLaw; + } else { + priorityPitchPitchLawCap = pitch_efcs_law::None; + } + + priorityPitchLateralLawCap = rtb_oppElacRollCapability; + } else { + priorityPitchPitchLawCap = pitch_efcs_law::None; + priorityPitchLateralLawCap = lateral_efcs_law::None; + } + + if (hasPriorityInRoll && rtb_isEngagedInRoll) { + rtb_oppElacRollCapability = rtb_lateralLawCapability; + } else if ((!rtb_ap_authorised) && rtb_isEngagedInRoll) { + rtb_oppElacRollCapability = lateral_efcs_law::None; + } + + if (rtb_isEngagedInRoll) { + if ((rtb_oppElacRollCapability == lateral_efcs_law::NormalLaw) && (priorityPitchPitchLawCap == pitch_efcs_law:: + NormalLaw) && (priorityPitchLateralLawCap == lateral_efcs_law::NormalLaw)) { + rtb_activeLateralLaw = lateral_efcs_law::NormalLaw; + } else { + rtb_activeLateralLaw = lateral_efcs_law::DirectLaw; + } + } else { + rtb_activeLateralLaw = lateral_efcs_law::None; + } + + if (rtb_isEngagedInPitch) { + if ((rtb_oppElacRollCapability == lateral_efcs_law::NormalLaw) && (priorityPitchPitchLawCap == pitch_efcs_law:: + NormalLaw) && (priorityPitchLateralLawCap == lateral_efcs_law::NormalLaw)) { + priorityPitchPitchLawCap = pitch_efcs_law::NormalLaw; + } else if ((rtb_oppElacRollCapability != lateral_efcs_law::NormalLaw) && (priorityPitchPitchLawCap == + pitch_efcs_law::NormalLaw)) { + priorityPitchPitchLawCap = pitch_efcs_law::AlternateLaw1; + } else if (priorityPitchPitchLawCap != pitch_efcs_law::NormalLaw) { + priorityPitchPitchLawCap = rtb_pitchLawCapability; + } else { + priorityPitchPitchLawCap = pitch_efcs_law::DirectLaw; + } + } else { + priorityPitchPitchLawCap = pitch_efcs_law::None; + } + + if (!ElacComputer_DWork.pRightStickDisabled) { + rtb_Sum2 = ElacComputer_U.in.analog_inputs.fo_pitch_stick_pos; + } else { + rtb_Sum2 = ElacComputer_P.Constant_Value_p; + } + + if (ElacComputer_DWork.pLeftStickDisabled) { + rtb_Switch3_p = ElacComputer_P.Constant_Value_p; + } else { + rtb_Switch3_p = ElacComputer_U.in.analog_inputs.capt_pitch_stick_pos; + } + + u0_0 = rtb_Sum2 + rtb_Switch3_p; + if (u0_0 > ElacComputer_P.Saturation_UpperSat_d) { + u0_0 = ElacComputer_P.Saturation_UpperSat_d; + } else if (u0_0 < ElacComputer_P.Saturation_LowerSat_h) { + u0_0 = ElacComputer_P.Saturation_LowerSat_h; + } + + ElacComputer_MATLABFunction_g(ElacComputer_B.in_flight != 0.0, ElacComputer_P.PulseNode_isRisingEdge_g, &rtb_y_aq, + &ElacComputer_DWork.sf_MATLABFunction_l0); + rtb_groundSettingHasPriority = ((ElacComputer_U.in.discrete_inputs.is_unit_1 && rtb_thsAvail && rtb_thsAvail_tmp) || + (ElacComputer_U.in.discrete_inputs.is_unit_2 && rtb_thsAvail && rtb_thsAvail_tmp && + ElacComputer_U.in.discrete_inputs.opp_axis_pitch_failure)); + rtb_Y_d = std::abs(ElacComputer_U.in.analog_inputs.ths_pos_deg); + ElacComputer_DWork.Memory_PreviousInput = ElacComputer_P.Logic_table[((((!rtb_groundSettingHasPriority) || (rtb_Y_d <= + ElacComputer_P.CompareToConstant_const_m) || ElacComputer_U.in.discrete_inputs.ths_override_active) + ( + static_cast(rtb_y_aq) << 1)) << 1) + ElacComputer_DWork.Memory_PreviousInput]; + rtb_NOT_k = (rtb_groundSettingHasPriority && ElacComputer_DWork.Memory_PreviousInput); + rtb_groundSettingHasPriority = ((rtb_isEngagedInPitch && (ElacComputer_B.in_flight != 0.0) && + ((priorityPitchPitchLawCap != ElacComputer_P.EnumeratedConstant_Value_i) && (!rtb_AND4))) || rtb_NOT_k); + ElacComputer_MATLABFunction_j(&ElacComputer_U.in.bus_inputs.sfcc_1_bus.slat_flap_system_status_word, + ElacComputer_P.BitfromLabel_bit_h2, &rtb_y_g); + rtb_y_aq = (rtb_y_g != 0U); + ElacComputer_MATLABFunction_j(&ElacComputer_U.in.bus_inputs.sfcc_1_bus.slat_flap_system_status_word, + ElacComputer_P.BitfromLabel1_bit_g, &rtb_y_g); + rtb_OR1_hu = (rtb_y_g != 0U); + ElacComputer_MATLABFunction_j(&ElacComputer_U.in.bus_inputs.sfcc_1_bus.slat_flap_system_status_word, + ElacComputer_P.BitfromLabel2_bit_n, &rtb_y_g); + rtb_y_k = (rtb_y_g != 0U); + ElacComputer_MATLABFunction_j(&ElacComputer_U.in.bus_inputs.sfcc_1_bus.slat_flap_system_status_word, + ElacComputer_P.BitfromLabel3_bit_g, &rtb_y_g); + rtb_AND1_h = (rtb_y_g != 0U); + ElacComputer_MATLABFunction_j(&ElacComputer_U.in.bus_inputs.sfcc_1_bus.slat_flap_system_status_word, + ElacComputer_P.BitfromLabel4_bit_e, &rtb_y_g); + rtb_AND3_b = (rtb_y_g != 0U); + ElacComputer_MATLABFunction_j(&ElacComputer_U.in.bus_inputs.sfcc_1_bus.slat_flap_system_status_word, + ElacComputer_P.BitfromLabel5_bit_a, &rtb_y_g); + ElacComputer_MATLABFunction_o(rtb_y_aq, rtb_OR1_hu, rtb_y_k, rtb_AND1_h, rtb_AND3_b, rtb_y_g != 0U, &rtb_handleIndex); + ElacComputer_RateLimiter_n(look2_binlxpw(static_cast(rtb_mach), rtb_handleIndex, + ElacComputer_P.alphamax_bp01Data, ElacComputer_P.alphamax_bp02Data, ElacComputer_P.alphamax_tableData, + ElacComputer_P.alphamax_maxIndex, 4U), ElacComputer_P.RateLimiterGenericVariableTs_up, + ElacComputer_P.RateLimiterGenericVariableTs_lo, ElacComputer_U.in.time.dt, ElacComputer_P.reset_Value, + &rtb_Switch3_p, &ElacComputer_DWork.sf_RateLimiter_n); + if (!ElacComputer_DWork.eventTime_not_empty_a) { + ElacComputer_DWork.eventTime_g = ElacComputer_U.in.time.simulation_time; + ElacComputer_DWork.eventTime_not_empty_a = true; + } + + if (rtb_OR6 || (ElacComputer_DWork.eventTime_g == 0.0)) { + ElacComputer_DWork.eventTime_g = ElacComputer_U.in.time.simulation_time; + } + + ElacComputer_RateLimiter_n(look2_binlxpw(static_cast(rtb_mach), rtb_handleIndex, + ElacComputer_P.alphaprotection_bp01Data, ElacComputer_P.alphaprotection_bp02Data, + ElacComputer_P.alphaprotection_tableData, ElacComputer_P.alphaprotection_maxIndex, 4U), + ElacComputer_P.RateLimiterGenericVariableTs1_up, ElacComputer_P.RateLimiterGenericVariableTs1_lo, + ElacComputer_U.in.time.dt, ElacComputer_P.reset_Value_j, &rtb_Y_d, &ElacComputer_DWork.sf_RateLimiter_m); + if (ElacComputer_U.in.time.simulation_time - ElacComputer_DWork.eventTime_g <= + ElacComputer_P.CompareToConstant_const_l) { + rtb_handleIndex = rtb_Switch3_p; + } else { + rtb_handleIndex = rtb_Y_d; + } + + ElacComputer_GetIASforMach4(static_cast(rtb_mach), ElacComputer_P.Constant6_Value_b, static_cast + (rtb_V_ias), &rtb_DataTypeConversion3_m); + rtb_DataTypeConversion3_m = std::fmin(ElacComputer_P.Constant5_Value_k, rtb_DataTypeConversion3_m); + ElacComputer_GetIASforMach4(static_cast(rtb_mach), ElacComputer_P.Constant8_Value_h, static_cast + (rtb_V_ias), &rtb_handleIndex_c); + rtb_BusAssignment_p_logic_high_speed_prot_hi_thresh_kn = std::fmin(ElacComputer_P.Constant7_Value_g, + rtb_handleIndex_c); + rtb_y_aq = rtb_NOT_k; + if (ElacComputer_DWork.is_active_c28_ElacComputer == 0U) { + ElacComputer_DWork.is_active_c28_ElacComputer = 1U; + ElacComputer_DWork.is_c28_ElacComputer = ElacComputer_IN_Landed; + rtb_ap_special_disc = 0; + } else { + switch (ElacComputer_DWork.is_c28_ElacComputer) { + case ElacComputer_IN_Flying: + if (rtb_raComputationValue < 100.0F) { + ElacComputer_DWork.is_c28_ElacComputer = ElacComputer_IN_Landing100ft; + rtb_ap_special_disc = 1; + } else if (rtb_OR6) { + ElacComputer_DWork.is_c28_ElacComputer = ElacComputer_IN_Landed; + rtb_ap_special_disc = 0; + } else { + rtb_ap_special_disc = 0; + } + break; + + case ElacComputer_IN_Landed: + if (!rtb_OR6) { + ElacComputer_DWork.is_c28_ElacComputer = ElacComputer_IN_Takeoff100ft; + rtb_ap_special_disc = 0; + } else { + rtb_ap_special_disc = 0; + } + break; + + case ElacComputer_IN_Landing100ft: + if (rtb_raComputationValue > 100.0F) { + ElacComputer_DWork.is_c28_ElacComputer = ElacComputer_IN_Flying; + rtb_ap_special_disc = 0; + } else if (rtb_OR6) { + ElacComputer_DWork.is_c28_ElacComputer = ElacComputer_IN_Landed; + rtb_ap_special_disc = 0; + } else { + rtb_ap_special_disc = 1; + } + break; + + default: + if (rtb_OR6) { + ElacComputer_DWork.is_c28_ElacComputer = ElacComputer_IN_Landed; + rtb_ap_special_disc = 0; + } else if (rtb_raComputationValue > 100.0F) { + ElacComputer_DWork.is_c28_ElacComputer = ElacComputer_IN_Flying; + rtb_ap_special_disc = 0; + } else { + rtb_ap_special_disc = 0; + } + break; + } + } + + rtb_Y_d = rtb_alpha - std::cos(ElacComputer_P.Gain1_Gain * rtb_phi) * rtb_Y; + if (!ElacComputer_DWork.eventTime_not_empty) { + ElacComputer_DWork.eventTime = ElacComputer_U.in.time.simulation_time; + ElacComputer_DWork.eventTime_not_empty = true; + } + + rtb_Sum2 = static_cast(rtb_V_ias) / rtb_mach; + if ((rtb_V_ias <= std::fmin(365.0, (look1_binlxpw(rtb_Y_d, ElacComputer_P.uDLookupTable_bp01Data, + ElacComputer_P.uDLookupTable_tableData, 3U) + 0.01) * rtb_Sum2)) || ((priorityPitchPitchLawCap != + pitch_efcs_law::NormalLaw) && (rtb_activeLateralLaw != lateral_efcs_law::NormalLaw)) || + (ElacComputer_DWork.eventTime == 0.0)) { + ElacComputer_DWork.eventTime = ElacComputer_U.in.time.simulation_time; + } + + rtb_NOT_k = ((priorityPitchPitchLawCap == pitch_efcs_law::NormalLaw) || (rtb_activeLateralLaw == lateral_efcs_law:: + NormalLaw)); + if (ElacComputer_U.in.discrete_inputs.ap_1_disengaged && ElacComputer_U.in.discrete_inputs.ap_2_disengaged && + (rtb_V_ias > std::fmin(look1_binlxpw(rtb_Y_d, ElacComputer_P.uDLookupTable1_bp01Data, + ElacComputer_P.uDLookupTable1_tableData, 3U), rtb_Sum2 * look1_binlxpw(rtb_Y_d, + ElacComputer_P.uDLookupTable2_bp01Data, ElacComputer_P.uDLookupTable2_tableData, 3U)))) { + ElacComputer_DWork.sProtActive = (rtb_NOT_k || ElacComputer_DWork.sProtActive); + } + + rtb_OR1_hu = (ElacComputer_U.in.discrete_inputs.ap_1_disengaged && ElacComputer_U.in.discrete_inputs.ap_2_disengaged); + ElacComputer_DWork.sProtActive = ((rtb_V_ias >= rtb_DataTypeConversion3_m) && (rtb_OR1_hu && rtb_NOT_k && + ElacComputer_DWork.sProtActive)); + rtb_NOT_k = ((priorityPitchPitchLawCap == pitch_efcs_law::NormalLaw) || (rtb_activeLateralLaw == lateral_efcs_law:: + NormalLaw)); + if (!ElacComputer_DWork.resetEventTime_not_empty) { + ElacComputer_DWork.resetEventTime = ElacComputer_U.in.time.simulation_time; + ElacComputer_DWork.resetEventTime_not_empty = true; + } + + if ((u0_0 >= -0.03125) || (rtb_Y >= rtb_Switch3_p) || (ElacComputer_DWork.resetEventTime == 0.0)) { + ElacComputer_DWork.resetEventTime = ElacComputer_U.in.time.simulation_time; + } + + ElacComputer_DWork.sProtActive_f = ((rtb_NOT_i && rtb_NOT_k && rtb_OR1_hu && (rtb_Y > rtb_handleIndex) && + (ElacComputer_U.in.time.monotonic_time > 10.0)) || ElacComputer_DWork.sProtActive_f); + ElacComputer_DWork.sProtActive_f = ((ElacComputer_U.in.time.simulation_time - ElacComputer_DWork.resetEventTime <= + 0.5) && (u0_0 >= -0.5) && ((rtb_raComputationValue >= 200.0F) || (u0_0 >= 0.5) || (rtb_Y >= rtb_handleIndex - 2.0)) + && rtb_NOT_i && rtb_NOT_k && ElacComputer_DWork.sProtActive_f); + rtb_NOT_k = ((rtb_NOT_i && (((rtb_ap_special_disc != 0) && (rtb_Y > rtb_Switch3_p)) || (rtb_Y > rtb_handleIndex + + 0.25)) && ((priorityPitchPitchLawCap == pitch_efcs_law::NormalLaw) || (rtb_activeLateralLaw == lateral_efcs_law:: + NormalLaw))) || (ElacComputer_U.in.time.simulation_time - ElacComputer_DWork.eventTime > 3.0) || + ElacComputer_DWork.sProtActive || ElacComputer_DWork.sProtActive_f); + rtb_ap_authorised = ((std::abs(u0_0) <= 0.5) && (std::abs(u0) <= 0.5) && ((std::abs + (ElacComputer_U.in.analog_inputs.rudder_pedal_pos) <= 0.4) && ((rtb_alpha <= 25.0F) && (rtb_alpha >= -13.0F) && + (abnormalCondition_tmp <= 45.0) && ((!hasPriorityInPitch) || rtb_thsAvail_tmp) && (rtb_ap_authorised || + canEngageInRoll) && (!rtb_NOT_k)))); + ElacComputer_Y.out.logic.protection_ap_disconnect = rtb_NOT_k; + ElacComputer_MATLABFunction_j(&ElacComputer_U.in.bus_inputs.sfcc_1_bus.slat_flap_actual_position_word, + ElacComputer_P.BitfromLabel_bit_e, &rtb_y_g); + rtb_NOT_i = (rtb_y_g == 0U); + ElacComputer_MATLABFunction(&ElacComputer_U.in.bus_inputs.sfcc_1_bus.slat_flap_actual_position_word, &rtb_NOT_k); + ElacComputer_MATLABFunction_j(&ElacComputer_U.in.bus_inputs.sfcc_2_bus.slat_flap_actual_position_word, + ElacComputer_P.BitfromLabel1_bit_d, &rtb_y_g); + ElacComputer_MATLABFunction(&ElacComputer_U.in.bus_inputs.sfcc_2_bus.slat_flap_actual_position_word, &rtb_y_k); + rtb_NOT_k = ((rtb_NOT_i && rtb_NOT_k) || ((rtb_y_g == 0U) && rtb_y_k)); + rtb_NOT_i = (rtb_NOT_k && ((leftAileronAvail && rightAileronAvail) || + ((!ElacComputer_U.in.discrete_inputs.opp_left_aileron_lost) && rightAileronAvail) || (leftAileronAvail && + (!ElacComputer_U.in.discrete_inputs.opp_right_aileron_lost)))); + rtb_aileronAntidroopActive = (rtb_aileronAntidroopActive && rtb_NOT_k && (rtb_alpha < 2.5F) && rtb_OR1_hu && + (rtb_activeLateralLaw == lateral_efcs_law::NormalLaw)); + abnormalCondition_tmp = rtb_DataTypeConversion3_m; + if (rtb_NOT_i) { + rtb_Sum2 = ElacComputer_P.Constant2_Value; + } else { + rtb_Sum2 = ElacComputer_P.Constant1_Value; + } + + ElacComputer_RateLimiter(rtb_Sum2, ElacComputer_P.RateLimiterVariableTs2_up, + ElacComputer_P.RateLimiterVariableTs2_lo, ElacComputer_U.in.time.dt, + ElacComputer_P.RateLimiterVariableTs2_InitialCondition, &rtb_Y_p, &ElacComputer_DWork.sf_RateLimiter); + if (rtb_aileronAntidroopActive) { + rtb_Sum2 = ElacComputer_P.Constant4_Value_a; + } else { + rtb_Sum2 = ElacComputer_P.Constant3_Value; + } + + ElacComputer_RateLimiter(rtb_Sum2, ElacComputer_P.RateLimiterVariableTs3_up, + ElacComputer_P.RateLimiterVariableTs3_lo, ElacComputer_U.in.time.dt, + ElacComputer_P.RateLimiterVariableTs3_InitialCondition, &rtb_Y_n, &ElacComputer_DWork.sf_RateLimiter_b); + rtb_Sum2 = rtb_Y_p + rtb_Y_n; + rtb_NOT_k = (rtb_AND2 || (static_cast(rtb_activeLateralLaw) != ElacComputer_P.CompareToConstant_const_m4)); + rtb_Y_p = ElacComputer_U.in.bus_inputs.fmgc_1_bus.delta_p_ail_cmd_deg.Data; + rtb_Y_n = ElacComputer_U.in.bus_inputs.fmgc_1_bus.delta_r_cmd_deg.Data; + rtb_OR1_hu = ((!ElacComputer_U.in.discrete_inputs.ap_1_disengaged) || + (!ElacComputer_U.in.discrete_inputs.ap_2_disengaged)); + LawMDLOBJ2.step(&ElacComputer_U.in.time.dt, &rtb_Y_d_tmp_tmp, &rtb_DataTypeConversion5, &rtb_DataTypeConversion8, + &rtb_xi_deg_m, &rtb_eta_trim_limit_lo_d, + &rtb_BusConversion_InsertedFor_BusAssignment_at_inport_8_BusCreator1_V_tas_kn, &rtb_handleIndex_f, + &u0, &ElacComputer_U.in.analog_inputs.rudder_pedal_pos, &rtb_OR6, &rtb_NOT_k, + &ElacComputer_DWork.sProtActive_f, &ElacComputer_DWork.sProtActive, &rtb_Y_p, &rtb_Y_n, &rtb_OR1_hu, + &rtb_xi_deg, &rtb_zeta_deg); + LawMDLOBJ1.step(&ElacComputer_U.in.time.dt, &u0, &rtb_xi_deg_m, &rtb_zeta_deg_f); + switch (static_cast(rtb_activeLateralLaw)) { + case 0: + rtb_xi_deg_m = rtb_xi_deg; + break; + + case 1: + break; + + default: + rtb_xi_deg_m = ElacComputer_P.Constant_Value_c; + break; + } + + if (rtb_rightAileronCrossCommandActive) { + rtb_Y_d = ElacComputer_U.in.bus_inputs.elac_opp_bus.aileron_command_deg.Data; + } else { + rtb_Y_d = rtb_xi_deg_m + rtb_Sum2; + } + + if (rtb_Y_d > ElacComputer_P.Saturation2_UpperSat) { + rtb_Y_d = ElacComputer_P.Saturation2_UpperSat; + } else if (rtb_Y_d < ElacComputer_P.Saturation2_LowerSat) { + rtb_Y_d = ElacComputer_P.Saturation2_LowerSat; + } + + ElacComputer_RateLimiter_a(rtb_Y_d, ElacComputer_P.RateLimiterGenericVariableTs_up_b, + ElacComputer_P.RateLimiterGenericVariableTs_lo_k, ElacComputer_U.in.time.dt, + ElacComputer_U.in.analog_inputs.right_aileron_pos_deg, (!rtb_rightAileronCrossCommandActive) && + (!rtb_isEngagedInRoll), &rtb_handleIndex_c, &ElacComputer_DWork.sf_RateLimiter_a); + if (rtb_leftAileronCrossCommandActive) { + rtb_Y_d = ElacComputer_U.in.bus_inputs.elac_opp_bus.aileron_command_deg.Data; + } else { + rtb_Y_d = ElacComputer_P.Gain_Gain * rtb_xi_deg_m + rtb_Sum2; + } + + if (rtb_Y_d > ElacComputer_P.Saturation1_UpperSat_g) { + rtb_Y_d = ElacComputer_P.Saturation1_UpperSat_g; + } else if (rtb_Y_d < ElacComputer_P.Saturation1_LowerSat_n) { + rtb_Y_d = ElacComputer_P.Saturation1_LowerSat_n; + } + + ElacComputer_RateLimiter_a(rtb_Y_d, ElacComputer_P.RateLimiterGenericVariableTs1_up_g, + ElacComputer_P.RateLimiterGenericVariableTs1_lo_c, ElacComputer_U.in.time.dt, + ElacComputer_U.in.analog_inputs.left_aileron_pos_deg, (!rtb_leftAileronCrossCommandActive) && + (!rtb_isEngagedInRoll), &rtb_DataTypeConversion3_m, &ElacComputer_DWork.sf_RateLimiter_p); + ElacComputer_MATLABFunction_j(&ElacComputer_U.in.bus_inputs.sfcc_1_bus.slat_flap_actual_position_word, + ElacComputer_P.BitfromLabel_bit_a2, &rtb_y_ee); + ElacComputer_MATLABFunction(&ElacComputer_U.in.bus_inputs.sfcc_1_bus.slat_flap_actual_position_word, &rtb_NOT_k); + rtb_NOT_k = ((rtb_y_ee != 0U) && rtb_NOT_k); + ElacComputer_MATLABFunction_j(&ElacComputer_U.in.bus_inputs.sfcc_2_bus.slat_flap_actual_position_word, + ElacComputer_P.BitfromLabel1_bit_p, &rtb_y_ee); + ElacComputer_MATLABFunction(&ElacComputer_U.in.bus_inputs.sfcc_2_bus.slat_flap_actual_position_word, &rtb_y_k); + if (rtb_NOT_k || ((rtb_y_ee != 0U) && rtb_y_k)) { + rtb_Y_d = rtb_xi_deg_m; + } else { + rtb_Y_p = std::abs(rtb_xi_deg_m) + ElacComputer_P.Bias_Bias; + if (rtb_Y_p > ElacComputer_P.Saturation_UpperSat) { + rtb_Y_p = ElacComputer_P.Saturation_UpperSat; + } else if (rtb_Y_p < ElacComputer_P.Saturation_LowerSat) { + rtb_Y_p = ElacComputer_P.Saturation_LowerSat; + } + + if (rtb_xi_deg_m < 0.0) { + rtb_Sum2 = -1.0; + } else { + rtb_Sum2 = (rtb_xi_deg_m > 0.0); + } + + rtb_Y_d = rtb_Y_p * rtb_Sum2 * ElacComputer_P.Gain2_Gain; + } + + rtb_xi_deg_m = ElacComputer_P.Gain1_Gain_b * rtb_Y_d; + switch (static_cast(rtb_activeLateralLaw)) { + case 0: + rtb_zeta_deg_f = rtb_zeta_deg; + break; + + case 1: + break; + + default: + rtb_zeta_deg_f = ElacComputer_P.Constant_Value_c; + break; + } + + rtb_Sum2 = rtb_DataTypeConversion3_m; + rtb_Y_p = rtb_handleIndex_c; + rtb_Y_n = ElacComputer_P.DiscreteDerivativeVariableTs_Gain * rtb_theta_dot; + ElacComputer_LagFilter((rtb_Y_n - ElacComputer_DWork.Delay_DSTATE) / ElacComputer_U.in.time.dt, + ElacComputer_P.LagFilter_C1_e, ElacComputer_U.in.time.dt, &rtb_Y_d, &ElacComputer_DWork.sf_LagFilter); + ElacComputer_MATLABFunction_j(&ElacComputer_U.in.bus_inputs.sfcc_1_bus.slat_flap_system_status_word, + ElacComputer_P.BitfromLabel_bit_p, &rtb_y_ee); + rtb_NOT_k = (rtb_y_ee != 0U); + ElacComputer_MATLABFunction_j(&ElacComputer_U.in.bus_inputs.sfcc_1_bus.slat_flap_system_status_word, + ElacComputer_P.BitfromLabel1_bit_h, &rtb_y_ee); + rtb_y_k = (rtb_y_ee != 0U); + ElacComputer_MATLABFunction_j(&ElacComputer_U.in.bus_inputs.sfcc_1_bus.slat_flap_system_status_word, + ElacComputer_P.BitfromLabel2_bit_f, &rtb_y_ee); + rtb_AND1_h = (rtb_y_ee != 0U); + ElacComputer_MATLABFunction_j(&ElacComputer_U.in.bus_inputs.sfcc_1_bus.slat_flap_system_status_word, + ElacComputer_P.BitfromLabel3_bit_c, &rtb_y_ee); + rtb_AND3_b = (rtb_y_ee != 0U); + ElacComputer_MATLABFunction_j(&ElacComputer_U.in.bus_inputs.sfcc_1_bus.slat_flap_system_status_word, + ElacComputer_P.BitfromLabel4_bit_n, &rtb_y_ee); + rtb_DataTypeConversion_m5 = (rtb_y_ee != 0U); + ElacComputer_MATLABFunction_j(&ElacComputer_U.in.bus_inputs.sfcc_1_bus.slat_flap_system_status_word, + ElacComputer_P.BitfromLabel5_bit_p, &rtb_y_ee); + ElacComputer_MATLABFunction_o(rtb_NOT_k, rtb_y_k, rtb_AND1_h, rtb_AND3_b, rtb_DataTypeConversion_m5, rtb_y_ee != 0U, + &rtb_handleIndex_c); + if ((ElacComputer_U.in.bus_inputs.sec_1_bus.thrust_lever_angle_1_deg.SSM == static_cast(SignStatusMatrix:: + NormalOperation)) && (ElacComputer_U.in.bus_inputs.sec_1_bus.thrust_lever_angle_2_deg.SSM == + static_cast(SignStatusMatrix::NormalOperation))) { + rtb_tla1 = ElacComputer_U.in.bus_inputs.sec_1_bus.thrust_lever_angle_1_deg.Data; + rtb_tla2 = ElacComputer_U.in.bus_inputs.sec_1_bus.thrust_lever_angle_2_deg.Data; + } else if ((ElacComputer_U.in.bus_inputs.sec_2_bus.thrust_lever_angle_1_deg.SSM == static_cast + (SignStatusMatrix::NormalOperation)) && + (ElacComputer_U.in.bus_inputs.sec_2_bus.thrust_lever_angle_2_deg.SSM == static_cast + (SignStatusMatrix::NormalOperation))) { + rtb_tla1 = ElacComputer_U.in.bus_inputs.sec_2_bus.thrust_lever_angle_1_deg.Data; + rtb_tla2 = ElacComputer_U.in.bus_inputs.sec_2_bus.thrust_lever_angle_2_deg.Data; + } else { + rtb_tla1 = 0.0F; + rtb_tla2 = 0.0F; + } + + rtb_DataTypeConversion3_m = rtb_tla1; + rtb_DataTypeConversion8 = rtb_tla2; + rtb_DataTypeConversion_o = (ElacComputer_B.in_flight != 0.0); + rtb_NOT_k = (rtb_AND2 || (static_cast(priorityPitchPitchLawCap) != ElacComputer_P.CompareToConstant_const_f)); + rtb_DataTypeConversion6_g = ElacComputer_U.in.bus_inputs.fmgc_1_bus.delta_q_cmd_deg.Data; + LawMDLOBJ5.step(&ElacComputer_U.in.time.dt, &rtb_BusAssignment_f_logic_ir_computation_data_n_z_g, &rtb_Y_d_tmp_tmp, + &rtb_DataTypeConversion5, &rtb_BusAssignment_f_logic_ir_computation_data_theta_dot_deg_s, &rtb_Y_d, + (const_cast(&ElacComputer_RGND)), &ElacComputer_U.in.analog_inputs.ths_pos_deg, &rtb_Y, + &rtb_eta_trim_limit_lo_d, + &rtb_BusConversion_InsertedFor_BusAssignment_at_inport_8_BusCreator1_V_tas_kn, &rtb_handleIndex_f, + &rtb_handleIndex_c, (const_cast(&ElacComputer_RGND)), (const_cast + (&ElacComputer_RGND)), &rtb_DataTypeConversion3_m, &rtb_DataTypeConversion8, + &ElacComputer_U.in.sim_data.tailstrike_protection_on, (const_cast(&ElacComputer_RGND)), + &u0_0, &rtb_OR6, &rtb_DataTypeConversion_o, &rtb_NOT_k, &ElacComputer_DWork.sProtActive_f, + &ElacComputer_DWork.sProtActive, &rtb_handleIndex, &rtb_Switch3_p, + &rtb_BusAssignment_p_logic_high_speed_prot_hi_thresh_kn, &abnormalCondition_tmp, + &rtb_DataTypeConversion6_g, &rtb_OR1_hu, &rtb_eta_deg, &rtb_eta_trim_dot_deg_s, + &rtb_eta_trim_limit_lo, &rtb_eta_trim_limit_up); + ElacComputer_MATLABFunction_j(&ElacComputer_U.in.bus_inputs.sfcc_1_bus.slat_flap_system_status_word, + ElacComputer_P.BitfromLabel_bit_n, &rtb_y_ee); + rtb_NOT_k = (rtb_y_ee != 0U); + ElacComputer_MATLABFunction_j(&ElacComputer_U.in.bus_inputs.sfcc_1_bus.slat_flap_system_status_word, + ElacComputer_P.BitfromLabel1_bit_h1, &rtb_y_ee); + rtb_y_k = (rtb_y_ee != 0U); + ElacComputer_MATLABFunction_j(&ElacComputer_U.in.bus_inputs.sfcc_1_bus.slat_flap_system_status_word, + ElacComputer_P.BitfromLabel2_bit_g, &rtb_y_ee); + rtb_AND1_h = (rtb_y_ee != 0U); + ElacComputer_MATLABFunction_j(&ElacComputer_U.in.bus_inputs.sfcc_1_bus.slat_flap_system_status_word, + ElacComputer_P.BitfromLabel3_bit_b, &rtb_y_ee); + rtb_AND3_b = (rtb_y_ee != 0U); + ElacComputer_MATLABFunction_j(&ElacComputer_U.in.bus_inputs.sfcc_1_bus.slat_flap_system_status_word, + ElacComputer_P.BitfromLabel4_bit_i, &rtb_y_ee); + rtb_DataTypeConversion_m5 = (rtb_y_ee != 0U); + ElacComputer_MATLABFunction_j(&ElacComputer_U.in.bus_inputs.sfcc_1_bus.slat_flap_system_status_word, + ElacComputer_P.BitfromLabel5_bit_l, &rtb_y_ee); + ElacComputer_MATLABFunction_o(rtb_NOT_k, rtb_y_k, rtb_AND1_h, rtb_AND3_b, rtb_DataTypeConversion_m5, rtb_y_ee != 0U, + &rtb_handleIndex_f); + rtb_Y_d = (ElacComputer_B.in_flight != 0.0); + rtb_NOT_k = (rtb_AND2 || ((static_cast(priorityPitchPitchLawCap) != ElacComputer_P.CompareToConstant2_const) + && (static_cast(priorityPitchPitchLawCap) != ElacComputer_P.CompareToConstant3_const))); + rtb_y_k = (priorityPitchPitchLawCap != ElacComputer_P.EnumeratedConstant_Value_b); + LawMDLOBJ3.step(&ElacComputer_U.in.time.dt, &rtb_BusAssignment_f_logic_ir_computation_data_n_z_g, &rtb_Y_d_tmp_tmp, + &rtb_DataTypeConversion5, &rtb_BusAssignment_f_logic_ir_computation_data_theta_dot_deg_s, ( + const_cast(&ElacComputer_RGND)), &ElacComputer_U.in.analog_inputs.ths_pos_deg, &rtb_eta_trim_limit_lo_d, + &rtb_BusConversion_InsertedFor_BusAssignment_at_inport_8_BusCreator1_mach, + &rtb_BusConversion_InsertedFor_BusAssignment_at_inport_8_BusCreator1_V_tas_kn, &rtb_handleIndex_f, ( + const_cast(&ElacComputer_RGND)), (const_cast(&ElacComputer_RGND)), &u0_0, &rtb_Y_d, &rtb_NOT_k, + &rtb_y_k, &rtb_eta_deg_o, &rtb_eta_trim_dot_deg_s_a, &rtb_eta_trim_limit_lo_h, + &rtb_eta_trim_limit_up_d); + LawMDLOBJ4.step(&ElacComputer_U.in.time.dt, &u0_0, &rtb_Y_d, &rtb_handleIndex_c, &rtb_eta_trim_limit_lo_d, + &rtb_DataTypeConversion3_m); + switch (static_cast(priorityPitchPitchLawCap)) { + case 0: + rtb_Y_d = rtb_eta_deg; + break; + + case 1: + case 2: + rtb_Y_d = rtb_eta_deg_o; + break; + + case 3: + break; + + default: + rtb_Y_d = ElacComputer_P.Constant_Value_a; + break; + } + + switch (static_cast(priorityPitchPitchLawCap)) { + case 0: + rtb_DataTypeConversion3_m = rtb_eta_trim_limit_up; + break; + + case 1: + case 2: + rtb_DataTypeConversion3_m = rtb_eta_trim_limit_up_d; + break; + + case 3: + break; + + default: + rtb_DataTypeConversion3_m = ElacComputer_P.Constant2_Value_l; + break; + } + + if (rtb_y_aq) { + rtb_handleIndex_c = ElacComputer_P.Gain_Gain_l * ElacComputer_DWork.Delay_DSTATE_b; + if (rtb_handleIndex_c > ElacComputer_P.Saturation_UpperSat_g) { + rtb_handleIndex_c = ElacComputer_P.Saturation_UpperSat_g; + } else if (rtb_handleIndex_c < ElacComputer_P.Saturation_LowerSat_o) { + rtb_handleIndex_c = ElacComputer_P.Saturation_LowerSat_o; + } + } else if (ElacComputer_U.in.discrete_inputs.ths_override_active) { + rtb_handleIndex_c = ElacComputer_P.Constant_Value_n; + } else { + switch (static_cast(priorityPitchPitchLawCap)) { + case 0: + rtb_handleIndex_c = rtb_eta_trim_dot_deg_s; + break; + + case 1: + case 2: + rtb_handleIndex_c = rtb_eta_trim_dot_deg_s_a; + break; + + case 3: + break; + + default: + rtb_handleIndex_c = ElacComputer_P.Constant_Value_a; + break; + } + } + + rtb_handleIndex_c = ElacComputer_P.DiscreteTimeIntegratorVariableTsLimit_Gain * rtb_handleIndex_c * + ElacComputer_U.in.time.dt; + ElacComputer_DWork.icLoad = ((!rtb_groundSettingHasPriority) || ElacComputer_DWork.icLoad); + if (ElacComputer_DWork.icLoad) { + ElacComputer_DWork.Delay_DSTATE_c = ElacComputer_U.in.analog_inputs.ths_pos_deg - rtb_handleIndex_c; + } + + ElacComputer_DWork.Delay_DSTATE_b = rtb_handleIndex_c + ElacComputer_DWork.Delay_DSTATE_c; + if (ElacComputer_DWork.Delay_DSTATE_b > rtb_DataTypeConversion3_m) { + ElacComputer_DWork.Delay_DSTATE_b = rtb_DataTypeConversion3_m; + } else { + switch (static_cast(priorityPitchPitchLawCap)) { + case 0: + rtb_eta_trim_limit_lo_d = rtb_eta_trim_limit_lo; + break; + + case 1: + case 2: + rtb_eta_trim_limit_lo_d = rtb_eta_trim_limit_lo_h; + break; + + case 3: + break; + + default: + rtb_eta_trim_limit_lo_d = ElacComputer_P.Constant3_Value_h; + break; + } + + if (ElacComputer_DWork.Delay_DSTATE_b < rtb_eta_trim_limit_lo_d) { + ElacComputer_DWork.Delay_DSTATE_b = rtb_eta_trim_limit_lo_d; + } + } + + rtb_DataTypeConversion_m5 = ((look1_binlxpw(static_cast(rtb_V_ias), ElacComputer_P.uDLookupTable_bp01Data_h, + ElacComputer_P.uDLookupTable_tableData_j, 6U) < std::abs(rtb_Y_d)) && rtb_isEngagedInPitch); + rtb_eta_trim_limit_lo_d = rtb_Y_d; + ElacComputer_MATLABFunction(&ElacComputer_U.in.bus_inputs.elac_opp_bus.elevator_double_pressurization_command_deg, + &rtb_y_k); + rtb_NOT_k = ((!rtb_isEngagedInPitch) && rtb_y_k); + if (rtb_NOT_k) { + rtb_Y_d = ElacComputer_U.in.bus_inputs.elac_opp_bus.elevator_double_pressurization_command_deg.Data; + } + + if ((rtb_NOT_k || rtb_isEngagedInPitch) && rtb_rightElevatorAvail) { + ElacComputer_Y.out.analog_outputs.right_elev_pos_order_deg = rtb_Y_d; + } else { + ElacComputer_Y.out.analog_outputs.right_elev_pos_order_deg = ElacComputer_P.Constant_Value_b; + } + + if ((rtb_isEngagedInPitch || rtb_NOT_k) && rtb_OR4) { + ElacComputer_Y.out.analog_outputs.left_elev_pos_order_deg = rtb_Y_d; + } else { + ElacComputer_Y.out.analog_outputs.left_elev_pos_order_deg = ElacComputer_P.Constant_Value_b; + } + + ElacComputer_MATLABFunction_j(&ElacComputer_U.in.bus_inputs.elac_opp_bus.discrete_status_word_1, + ElacComputer_P.BitfromLabel_bit_p3, &rtb_y_g); + rtb_AND3_b = (rtb_y_g != 0U); + if (ElacComputer_U.in.discrete_inputs.is_unit_2) { + rtb_Switch1_g_0 = &ElacComputer_U.in.bus_inputs.sec_1_bus.discrete_status_word_1; + } else { + rtb_Switch1_g_0 = &ElacComputer_U.in.bus_inputs.sec_2_bus.discrete_status_word_1; + } + + ElacComputer_MATLABFunction_j(rtb_Switch1_g_0, ElacComputer_P.BitfromLabel2_bit_j, &rtb_y_g); + rtb_AND3_b = (rtb_DataTypeConversion_m5 && (rtb_AND3_b || (rtb_y_g != 0U))); + ElacComputer_MATLABFunction_j(&ElacComputer_U.in.bus_inputs.elac_opp_bus.discrete_status_word_1, + ElacComputer_P.BitfromLabel1_bit_i, &rtb_y_g); + rtb_AND1_h = (rtb_y_g != 0U); + ElacComputer_MATLABFunction_j(rtb_Switch1_g_0, ElacComputer_P.BitfromLabel3_bit_mo, &rtb_y_g); + rtb_AND1_h = ((rtb_AND1_h || (rtb_y_g != 0U)) && rtb_DataTypeConversion_m5); + ElacComputer_MATLABFunction_c(rtb_BusAssignment_n_logic_is_yellow_hydraulic_power_avail || rtb_OR7 || rtb_OR, + ElacComputer_U.in.time.dt, ElacComputer_P.ConfirmNode_isRisingEdge_f, ElacComputer_P.ConfirmNode_timeDelay_p, + &rtb_y_k, &ElacComputer_DWork.sf_MATLABFunction_fb); + ElacComputer_Y.out.bus_outputs.elevator_double_pressurization_command_deg.SSM = static_cast + (ElacComputer_P.EnumeratedConstant_Value); + if (ElacComputer_P.EnumeratedConstant2_Value == rtb_activeLateralLaw) { + rtb_ap_special_disc = static_cast(ElacComputer_P.EnumeratedConstant1_Value); + ElacComputer_Y.out.bus_outputs.yaw_damper_command_deg.Data = static_cast(rtb_zeta_deg_f); + } else { + rtb_ap_special_disc = static_cast(ElacComputer_P.EnumeratedConstant_Value); + ElacComputer_Y.out.bus_outputs.yaw_damper_command_deg.Data = static_cast(ElacComputer_P.Constant7_Value); + } + + if (rtb_DataTypeConversion_m5) { + ElacComputer_Y.out.bus_outputs.elevator_double_pressurization_command_deg.SSM = static_cast + (ElacComputer_P.EnumeratedConstant1_Value); + ElacComputer_Y.out.bus_outputs.elevator_double_pressurization_command_deg.Data = static_cast + (rtb_eta_trim_limit_lo_d); + } else { + ElacComputer_Y.out.bus_outputs.elevator_double_pressurization_command_deg.Data = static_cast + (ElacComputer_P.Constant8_Value); + } + + rtb_VectorConcatenate[0] = ElacComputer_U.in.discrete_inputs.l_ail_servo_failed; + rtb_VectorConcatenate[1] = ElacComputer_U.in.discrete_inputs.r_ail_servo_failed; + rtb_VectorConcatenate[2] = ElacComputer_U.in.discrete_inputs.l_elev_servo_failed; + rtb_VectorConcatenate[3] = ElacComputer_U.in.discrete_inputs.r_elev_servo_failed; + rtb_VectorConcatenate[4] = leftAileronAvail; + rtb_VectorConcatenate[5] = rightAileronAvail; + rtb_VectorConcatenate[6] = rtb_OR4; + rtb_VectorConcatenate[7] = rtb_rightElevatorAvail; + rtb_VectorConcatenate[8] = rtb_isEngagedInPitch; + rtb_VectorConcatenate[9] = rtb_isEngagedInRoll; + rtb_VectorConcatenate[10] = !rtb_thsAvail_tmp; + rtb_VectorConcatenate[11] = !canEngageInRoll; + rtb_VectorConcatenate[12] = ((priorityPitchPitchLawCap == pitch_efcs_law::NormalLaw) || (priorityPitchPitchLawCap == + pitch_efcs_law::AlternateLaw2)); + rtb_VectorConcatenate[13] = ((priorityPitchPitchLawCap == pitch_efcs_law::AlternateLaw1) || + (priorityPitchPitchLawCap == pitch_efcs_law::AlternateLaw2)); + rtb_VectorConcatenate[14] = (priorityPitchPitchLawCap == pitch_efcs_law::DirectLaw); + ElacComputer_LateralLawCaptoBits(rtb_activeLateralLaw, &rtb_VectorConcatenate[15], &rtb_VectorConcatenate[16]); + ElacComputer_MATLABFunction_j(&ElacComputer_U.in.bus_inputs.sec_1_bus.discrete_status_word_1, + ElacComputer_P.BitfromLabel_bit_es, &rtb_y_g); + rtb_VectorConcatenate[17] = ((rtb_activeLateralLaw == lateral_efcs_law::NormalLaw) || rtb_AND4 || ((rtb_y_g == 0U) && + (ElacComputer_U.in.bus_inputs.sec_1_bus.discrete_status_word_1.SSM == static_cast(SignStatusMatrix:: + NormalOperation)))); + rtb_VectorConcatenate[18] = ((rtb_activeLateralLaw == lateral_efcs_law::NormalLaw) || rtb_AND4); + ElacComputer_MATLABFunction_cw(rtb_VectorConcatenate, &ElacComputer_Y.out.bus_outputs.discrete_status_word_1.Data); + rtb_VectorConcatenate_a[0] = ((rtb_pitchLawCapability == pitch_efcs_law::NormalLaw) || (rtb_pitchLawCapability == + pitch_efcs_law::DirectLaw)); + rtb_VectorConcatenate_a[1] = ((rtb_pitchLawCapability == pitch_efcs_law::AlternateLaw1) || (rtb_pitchLawCapability == + pitch_efcs_law::AlternateLaw2) || (rtb_pitchLawCapability == pitch_efcs_law::DirectLaw)); + ElacComputer_LateralLawCaptoBits(rtb_lateralLawCapability, &rtb_VectorConcatenate_a[2], &rtb_VectorConcatenate_a[3]); + rtb_VectorConcatenate_a[4] = ElacComputer_P.Constant9_Value; + rtb_VectorConcatenate_a[5] = ElacComputer_P.Constant9_Value; + rtb_VectorConcatenate_a[6] = ElacComputer_DWork.pLeftStickDisabled; + rtb_VectorConcatenate_a[7] = ElacComputer_DWork.pRightStickDisabled; + rtb_VectorConcatenate_a[8] = rtb_OR1; + rtb_VectorConcatenate_a[9] = rtb_OR3; + rtb_VectorConcatenate_a[10] = rtb_NOT_i; + rtb_VectorConcatenate_a[11] = rtb_OR1_hu; + rtb_VectorConcatenate_a[12] = ElacComputer_DWork.sProtActive_f; + rtb_VectorConcatenate_a[13] = ElacComputer_P.Constant10_Value; + rtb_VectorConcatenate_a[14] = ElacComputer_P.Constant10_Value; + rtb_VectorConcatenate_a[15] = ElacComputer_P.Constant10_Value; + rtb_VectorConcatenate_a[16] = ElacComputer_P.Constant10_Value; + rtb_VectorConcatenate_a[17] = ElacComputer_P.Constant10_Value; + rtb_VectorConcatenate_a[18] = ElacComputer_P.Constant10_Value; + ElacComputer_MATLABFunction_cw(rtb_VectorConcatenate_a, &ElacComputer_Y.out.bus_outputs.discrete_status_word_2.Data); + ElacComputer_Y.out.data = ElacComputer_U.in; + ElacComputer_Y.out.laws.lateral_law_outputs.left_aileron_command_deg = rtb_Sum2; + ElacComputer_Y.out.laws.lateral_law_outputs.right_aileron_command_deg = rtb_Y_p; + ElacComputer_Y.out.laws.lateral_law_outputs.roll_spoiler_command_deg = rtb_xi_deg_m; + ElacComputer_Y.out.laws.lateral_law_outputs.yaw_damper_command_deg = rtb_zeta_deg_f; + ElacComputer_Y.out.laws.pitch_law_outputs.elevator_command_deg = rtb_eta_trim_limit_lo_d; + ElacComputer_Y.out.laws.pitch_law_outputs.ths_command_deg = ElacComputer_DWork.Delay_DSTATE_b; + ElacComputer_Y.out.laws.pitch_law_outputs.elevator_double_pressurization_active = rtb_DataTypeConversion_m5; + ElacComputer_Y.out.logic.on_ground = rtb_OR6; + ElacComputer_Y.out.logic.pitch_law_in_flight = (ElacComputer_B.in_flight != 0.0); + ElacComputer_Y.out.logic.tracking_mode_on = rtb_AND2; + ElacComputer_Y.out.logic.lateral_law_capability = rtb_lateralLawCapability; + ElacComputer_Y.out.logic.active_lateral_law = rtb_activeLateralLaw; + ElacComputer_Y.out.logic.pitch_law_capability = rtb_pitchLawCapability; + ElacComputer_Y.out.logic.active_pitch_law = priorityPitchPitchLawCap; + ElacComputer_Y.out.logic.abnormal_condition_law_active = rtb_AND4; + ElacComputer_Y.out.logic.is_engaged_in_pitch = rtb_isEngagedInPitch; + ElacComputer_Y.out.logic.can_engage_in_pitch = rtb_thsAvail_tmp; + ElacComputer_Y.out.logic.has_priority_in_pitch = hasPriorityInPitch; + ElacComputer_Y.out.logic.left_elevator_avail = rtb_OR4; + ElacComputer_Y.out.logic.right_elevator_avail = rtb_rightElevatorAvail; + ElacComputer_Y.out.logic.ths_avail = rtb_thsAvail; + ElacComputer_Y.out.logic.ths_active_commanded = rtb_groundSettingHasPriority; + ElacComputer_Y.out.logic.ths_ground_setting_active = rtb_y_aq; + ElacComputer_Y.out.logic.is_engaged_in_roll = rtb_isEngagedInRoll; + ElacComputer_Y.out.logic.can_engage_in_roll = canEngageInRoll; + ElacComputer_Y.out.logic.has_priority_in_roll = hasPriorityInRoll; + ElacComputer_Y.out.logic.left_aileron_crosscommand_active = rtb_leftAileronCrossCommandActive; + ElacComputer_Y.out.logic.right_aileron_crosscommand_active = rtb_rightAileronCrossCommandActive; + ElacComputer_Y.out.logic.left_aileron_avail = leftAileronAvail; + ElacComputer_Y.out.logic.right_aileron_avail = rightAileronAvail; + ElacComputer_Y.out.logic.aileron_droop_active = rtb_NOT_i; + ElacComputer_Y.out.logic.aileron_antidroop_active = rtb_aileronAntidroopActive; + ElacComputer_Y.out.logic.is_yellow_hydraulic_power_avail = rtb_BusAssignment_n_logic_is_yellow_hydraulic_power_avail; + ElacComputer_Y.out.logic.is_blue_hydraulic_power_avail = rtb_OR7; + ElacComputer_Y.out.logic.is_green_hydraulic_power_avail = rtb_OR; + ElacComputer_Y.out.logic.left_sidestick_disabled = ElacComputer_DWork.pLeftStickDisabled; + ElacComputer_Y.out.logic.right_sidestick_disabled = ElacComputer_DWork.pRightStickDisabled; + ElacComputer_Y.out.logic.left_sidestick_priority_locked = rtb_OR1; + ElacComputer_Y.out.logic.right_sidestick_priority_locked = rtb_OR3; + ElacComputer_Y.out.logic.total_sidestick_pitch_command = u0_0; + ElacComputer_Y.out.logic.total_sidestick_roll_command = u0; + ElacComputer_Y.out.logic.ap_authorised = rtb_ap_authorised; + ElacComputer_Y.out.logic.high_alpha_prot_active = ElacComputer_DWork.sProtActive_f; + ElacComputer_Y.out.logic.alpha_prot_deg = rtb_handleIndex; + ElacComputer_Y.out.logic.alpha_max_deg = rtb_Switch3_p; + ElacComputer_Y.out.logic.high_speed_prot_active = ElacComputer_DWork.sProtActive; + ElacComputer_Y.out.logic.high_speed_prot_lo_thresh_kn = abnormalCondition_tmp; + ElacComputer_Y.out.logic.high_speed_prot_hi_thresh_kn = rtb_BusAssignment_p_logic_high_speed_prot_hi_thresh_kn; + ElacComputer_Y.out.logic.double_adr_failure = rtb_doubleAdrFault; + ElacComputer_Y.out.logic.triple_adr_failure = rtb_tripleAdrFault; + ElacComputer_Y.out.logic.cas_or_mach_disagree = ElacComputer_P.Constant1_Value_b; + ElacComputer_Y.out.logic.alpha_disagree = ElacComputer_P.Constant1_Value_b; + ElacComputer_Y.out.logic.double_ir_failure = rtb_doubleIrFault; + ElacComputer_Y.out.logic.triple_ir_failure = rtb_tripleIrFault; + ElacComputer_Y.out.logic.ir_failure_not_self_detected = ElacComputer_P.Constant_Value_ad; + ElacComputer_Y.out.logic.adr_computation_data.V_ias_kn = rtb_V_ias; + ElacComputer_Y.out.logic.adr_computation_data.V_tas_kn = rtb_V_tas; + ElacComputer_Y.out.logic.adr_computation_data.mach = rtb_mach; + ElacComputer_Y.out.logic.adr_computation_data.alpha_deg = rtb_Y; + ElacComputer_Y.out.logic.ir_computation_data.theta_deg = rtb_alpha; + ElacComputer_Y.out.logic.ir_computation_data.phi_deg = rtb_phi; + ElacComputer_Y.out.logic.ir_computation_data.q_deg_s = rtb_q; + ElacComputer_Y.out.logic.ir_computation_data.r_deg_s = rtb_r; + ElacComputer_Y.out.logic.ir_computation_data.n_x_g = rtb_n_x; + ElacComputer_Y.out.logic.ir_computation_data.n_y_g = rtb_n_y; + ElacComputer_Y.out.logic.ir_computation_data.n_z_g = rtb_n_z; + ElacComputer_Y.out.logic.ir_computation_data.theta_dot_deg_s = rtb_theta_dot; + ElacComputer_Y.out.logic.ir_computation_data.phi_dot_deg_s = rtb_phi_dot; + ElacComputer_Y.out.logic.ra_computation_data_ft = rtb_raComputationValue; + ElacComputer_Y.out.logic.dual_ra_failure = rtb_AND1; + ElacComputer_Y.out.discrete_outputs.pitch_axis_ok = rtb_thsAvail_tmp; + ElacComputer_Y.out.discrete_outputs.left_aileron_ok = leftAileronAvail; + ElacComputer_Y.out.discrete_outputs.right_aileron_ok = rightAileronAvail; + ElacComputer_Y.out.discrete_outputs.digital_output_validated = ElacComputer_P.Constant1_Value_e; + ElacComputer_Y.out.discrete_outputs.ap_1_authorised = rtb_ap_authorised; + ElacComputer_Y.out.discrete_outputs.ap_2_authorised = rtb_ap_authorised; + ElacComputer_Y.out.discrete_outputs.left_aileron_active_mode = ((rtb_isEngagedInRoll || + rtb_leftAileronCrossCommandActive) && leftAileronAvail); + ElacComputer_Y.out.discrete_outputs.right_aileron_active_mode = ((rtb_isEngagedInRoll || + rtb_rightAileronCrossCommandActive) && rightAileronAvail); + ElacComputer_Y.out.discrete_outputs.left_elevator_damping_mode = (rtb_isEngagedInPitch && rtb_OR4 && (!rtb_AND3_b)); + ElacComputer_Y.out.discrete_outputs.right_elevator_damping_mode = (rtb_isEngagedInPitch && rtb_rightElevatorAvail && + (!rtb_AND1_h)); + ElacComputer_Y.out.discrete_outputs.ths_active = (rtb_groundSettingHasPriority && rtb_thsAvail); + ElacComputer_Y.out.discrete_outputs.batt_power_supply = rtb_y_k; + if (rtb_groundSettingHasPriority && rtb_thsAvail) { + ElacComputer_Y.out.analog_outputs.ths_pos_order = ElacComputer_DWork.Delay_DSTATE_b; + } else { + ElacComputer_Y.out.analog_outputs.ths_pos_order = ElacComputer_P.Constant_Value_b; + } + + if ((rtb_isEngagedInRoll || rtb_leftAileronCrossCommandActive) && leftAileronAvail) { + ElacComputer_Y.out.analog_outputs.left_aileron_pos_order = rtb_Sum2; + } else { + ElacComputer_Y.out.analog_outputs.left_aileron_pos_order = ElacComputer_P.Constant_Value_b; + } + + if ((rtb_isEngagedInRoll || rtb_rightAileronCrossCommandActive) && rightAileronAvail) { + ElacComputer_Y.out.analog_outputs.right_aileron_pos_order = rtb_Y_p; + } else { + ElacComputer_Y.out.analog_outputs.right_aileron_pos_order = ElacComputer_P.Constant_Value_b; + } + + if (ElacComputer_U.in.discrete_inputs.l_ail_servo_failed) { + ElacComputer_Y.out.bus_outputs.left_aileron_position_deg.SSM = static_cast + (ElacComputer_P.EnumeratedConstant_Value); + ElacComputer_Y.out.bus_outputs.left_aileron_position_deg.Data = static_cast + (ElacComputer_P.Constant_Value_j); + } else { + ElacComputer_Y.out.bus_outputs.left_aileron_position_deg.SSM = static_cast + (ElacComputer_P.EnumeratedConstant1_Value); + ElacComputer_Y.out.bus_outputs.left_aileron_position_deg.Data = static_cast + (ElacComputer_U.in.analog_inputs.left_aileron_pos_deg); + } + + if (ElacComputer_U.in.discrete_inputs.r_ail_servo_failed) { + ElacComputer_Y.out.bus_outputs.right_aileron_position_deg.SSM = static_cast + (ElacComputer_P.EnumeratedConstant_Value); + ElacComputer_Y.out.bus_outputs.right_aileron_position_deg.Data = static_cast + (ElacComputer_P.Constant1_Value_d); + } else { + ElacComputer_Y.out.bus_outputs.right_aileron_position_deg.SSM = static_cast + (ElacComputer_P.EnumeratedConstant1_Value); + ElacComputer_Y.out.bus_outputs.right_aileron_position_deg.Data = static_cast + (ElacComputer_U.in.analog_inputs.right_aileron_pos_deg); + } + + if (ElacComputer_U.in.discrete_inputs.l_elev_servo_failed) { + ElacComputer_Y.out.bus_outputs.left_elevator_position_deg.SSM = static_cast + (ElacComputer_P.EnumeratedConstant_Value); + ElacComputer_Y.out.bus_outputs.left_elevator_position_deg.Data = static_cast + (ElacComputer_P.Constant2_Value_b); + } else { + ElacComputer_Y.out.bus_outputs.left_elevator_position_deg.SSM = static_cast + (ElacComputer_P.EnumeratedConstant1_Value); + ElacComputer_Y.out.bus_outputs.left_elevator_position_deg.Data = static_cast + (ElacComputer_U.in.analog_inputs.left_elevator_pos_deg); + } + + if (ElacComputer_U.in.discrete_inputs.r_elev_servo_failed) { + ElacComputer_Y.out.bus_outputs.right_elevator_position_deg.SSM = static_cast + (ElacComputer_P.EnumeratedConstant_Value); + ElacComputer_Y.out.bus_outputs.right_elevator_position_deg.Data = static_cast + (ElacComputer_P.Constant3_Value_f); + } else { + ElacComputer_Y.out.bus_outputs.right_elevator_position_deg.SSM = static_cast + (ElacComputer_P.EnumeratedConstant1_Value); + ElacComputer_Y.out.bus_outputs.right_elevator_position_deg.Data = static_cast + (ElacComputer_U.in.analog_inputs.right_elevator_pos_deg); + } + + if (ElacComputer_U.in.discrete_inputs.ths_motor_fault) { + ElacComputer_Y.out.bus_outputs.ths_position_deg.SSM = static_cast + (ElacComputer_P.EnumeratedConstant_Value); + ElacComputer_Y.out.bus_outputs.ths_position_deg.Data = static_cast(ElacComputer_P.Constant4_Value_i); + } else { + ElacComputer_Y.out.bus_outputs.ths_position_deg.SSM = static_cast + (ElacComputer_P.EnumeratedConstant1_Value); + ElacComputer_Y.out.bus_outputs.ths_position_deg.Data = static_cast + (ElacComputer_U.in.analog_inputs.ths_pos_deg); + } + + ElacComputer_Y.out.bus_outputs.left_sidestick_pitch_command_deg.SSM = static_cast + (ElacComputer_P.EnumeratedConstant1_Value); + ElacComputer_Y.out.bus_outputs.left_sidestick_pitch_command_deg.Data = ElacComputer_P.Gain_Gain_b * + static_cast(ElacComputer_U.in.analog_inputs.capt_pitch_stick_pos); + ElacComputer_Y.out.bus_outputs.right_sidestick_pitch_command_deg.SSM = static_cast + (ElacComputer_P.EnumeratedConstant1_Value); + ElacComputer_Y.out.bus_outputs.right_sidestick_pitch_command_deg.Data = ElacComputer_P.Gain1_Gain_f * static_cast< + real32_T>(ElacComputer_U.in.analog_inputs.fo_pitch_stick_pos); + ElacComputer_Y.out.bus_outputs.left_sidestick_roll_command_deg.SSM = static_cast + (ElacComputer_P.EnumeratedConstant1_Value); + ElacComputer_Y.out.bus_outputs.left_sidestick_roll_command_deg.Data = ElacComputer_P.Gain2_Gain_c * + static_cast(ElacComputer_U.in.analog_inputs.capt_roll_stick_pos); + ElacComputer_Y.out.bus_outputs.right_sidestick_roll_command_deg.SSM = static_cast + (ElacComputer_P.EnumeratedConstant1_Value); + ElacComputer_Y.out.bus_outputs.right_sidestick_roll_command_deg.Data = ElacComputer_P.Gain3_Gain * + static_cast(ElacComputer_U.in.analog_inputs.fo_roll_stick_pos); + ElacComputer_Y.out.bus_outputs.rudder_pedal_position_deg.SSM = static_cast + (ElacComputer_P.EnumeratedConstant1_Value); + ElacComputer_Y.out.bus_outputs.rudder_pedal_position_deg.Data = ElacComputer_P.Gain4_Gain * static_cast + (ElacComputer_U.in.analog_inputs.rudder_pedal_pos); + if ((rtb_OR_e1 || rtb_OR1_me) && rtb_isEngagedInRoll) { + ElacComputer_Y.out.bus_outputs.aileron_command_deg.SSM = static_cast + (ElacComputer_P.EnumeratedConstant1_Value); + if (!leftAileronAvail) { + ElacComputer_Y.out.bus_outputs.aileron_command_deg.Data = static_cast(rtb_Sum2); + } else { + ElacComputer_Y.out.bus_outputs.aileron_command_deg.Data = static_cast(rtb_Y_p); + } + } else { + ElacComputer_Y.out.bus_outputs.aileron_command_deg.SSM = static_cast + (ElacComputer_P.EnumeratedConstant_Value); + ElacComputer_Y.out.bus_outputs.aileron_command_deg.Data = static_cast(ElacComputer_P.Constant5_Value); + } + + if (static_cast(rtb_isEngagedInRoll) > ElacComputer_P.Switch13_Threshold) { + ElacComputer_Y.out.bus_outputs.roll_spoiler_command_deg.SSM = static_cast + (ElacComputer_P.EnumeratedConstant1_Value); + } else { + ElacComputer_Y.out.bus_outputs.roll_spoiler_command_deg.SSM = static_cast + (ElacComputer_P.EnumeratedConstant_Value); + } + + if (static_cast(rtb_isEngagedInRoll) > ElacComputer_P.Switch12_Threshold) { + ElacComputer_Y.out.bus_outputs.roll_spoiler_command_deg.Data = static_cast(rtb_xi_deg_m); + } else { + ElacComputer_Y.out.bus_outputs.roll_spoiler_command_deg.Data = static_cast + (ElacComputer_P.Constant6_Value); + } + + ElacComputer_Y.out.bus_outputs.yaw_damper_command_deg.SSM = static_cast(rtb_ap_special_disc); + ElacComputer_Y.out.bus_outputs.speedbrake_extension_deg = + ElacComputer_U.in.bus_inputs.sec_1_bus.speed_brake_command_deg; + ElacComputer_Y.out.bus_outputs.discrete_status_word_1.SSM = static_cast + (ElacComputer_P.EnumeratedConstant1_Value); + ElacComputer_Y.out.bus_outputs.discrete_status_word_2.SSM = static_cast + (ElacComputer_P.EnumeratedConstant1_Value); + ElacComputer_DWork.Delay_DSTATE_cc = rtb_OR1; + ElacComputer_DWork.Delay1_DSTATE = rtb_OR3; + ElacComputer_DWork.Delay_DSTATE = rtb_Y_n; + ElacComputer_DWork.icLoad = false; + ElacComputer_DWork.Delay_DSTATE_c = ElacComputer_DWork.Delay_DSTATE_b; + } else { + ElacComputer_DWork.Runtime_MODE = false; + } +} + +void ElacComputer::initialize() +{ + ElacComputer_DWork.Delay_DSTATE_cc = ElacComputer_P.Delay_InitialCondition_c; + ElacComputer_DWork.Delay1_DSTATE = ElacComputer_P.Delay1_InitialCondition; + ElacComputer_DWork.Memory_PreviousInput = ElacComputer_P.SRFlipFlop_initial_condition; + ElacComputer_DWork.Delay_DSTATE = ElacComputer_P.DiscreteDerivativeVariableTs_InitialCondition; + ElacComputer_DWork.Delay_DSTATE_b = ElacComputer_P.Delay_InitialCondition; + ElacComputer_DWork.icLoad = true; + LawMDLOBJ2.init(); + LawMDLOBJ5.init(); + LawMDLOBJ3.init(); + ElacComputer_Y.out = ElacComputer_P.out_Y0; +} + +void ElacComputer::terminate() +{ +} + +ElacComputer::ElacComputer(): + ElacComputer_U(), + ElacComputer_Y(), + ElacComputer_B(), + ElacComputer_DWork() +{ +} + +ElacComputer::~ElacComputer() +{ +} diff --git a/hdw-a339x/src/wasm/fbw_a320/src/model/ElacComputer.h b/hdw-a339x/src/wasm/fbw_a320/src/model/ElacComputer.h new file mode 100644 index 000000000..ed85ef06e --- /dev/null +++ b/hdw-a339x/src/wasm/fbw_a320/src/model/ElacComputer.h @@ -0,0 +1,377 @@ +#ifndef RTW_HEADER_ElacComputer_h_ +#define RTW_HEADER_ElacComputer_h_ +#include "rtwtypes.h" +#include "ElacComputer_types.h" +#include "LateralNormalLaw.h" +#include "LateralDirectLaw.h" +#include "PitchNormalLaw.h" +#include "PitchAlternateLaw.h" +#include "PitchDirectLaw.h" + +extern const real_T ElacComputer_RGND; +extern base_elac_logic_outputs rtP_elac_logic_output_MATLABStruct; +extern base_elac_analog_outputs rtP_elac_analog_output_MATLABStruct; +extern base_elac_discrete_outputs rtP_elac_discrete_output_MATLABStruct; +class ElacComputer final +{ + public: + struct rtDW_RateLimiter_ElacComputer_T { + real_T pY; + boolean_T pY_not_empty; + }; + + struct rtDW_RateLimiter_ElacComputer_g_T { + real_T pY; + boolean_T pY_not_empty; + }; + + struct rtDW_LagFilter_ElacComputer_T { + real_T pY; + real_T pU; + boolean_T pY_not_empty; + boolean_T pU_not_empty; + }; + + struct rtDW_MATLABFunction_ElacComputer_kz_T { + real_T timeSinceCondition; + boolean_T output; + }; + + struct rtDW_MATLABFunction_ElacComputer_o_T { + boolean_T output; + }; + + struct rtDW_RateLimiter_ElacComputer_b_T { + real_T pY; + boolean_T pY_not_empty; + }; + + struct rtDW_MATLABFunction_ElacComputer_b_T { + boolean_T previousInput; + boolean_T previousInput_not_empty; + }; + + struct BlockIO_ElacComputer_T { + real_T in_flight; + }; + + struct D_Work_ElacComputer_T { + real_T Delay_DSTATE; + real_T Delay_DSTATE_b; + real_T Delay_DSTATE_c; + real_T configFullEventTime; + real_T eventTime; + real_T resetEventTime; + real_T eventTime_g; + real_T on_ground_time; + boolean_T Delay_DSTATE_cc; + boolean_T Delay1_DSTATE; + uint8_T is_active_c28_ElacComputer; + uint8_T is_c28_ElacComputer; + uint8_T is_active_c30_ElacComputer; + uint8_T is_c30_ElacComputer; + boolean_T Memory_PreviousInput; + boolean_T icLoad; + boolean_T pLeftStickDisabled; + boolean_T pRightStickDisabled; + boolean_T configFullEventTime_not_empty; + boolean_T ra1CoherenceRejected; + boolean_T ra2CoherenceRejected; + boolean_T sProtActive; + boolean_T eventTime_not_empty; + boolean_T resetEventTime_not_empty; + boolean_T sProtActive_f; + boolean_T eventTime_not_empty_a; + boolean_T abnormalConditionWasActive; + boolean_T Runtime_MODE; + rtDW_MATLABFunction_ElacComputer_kz_T sf_MATLABFunction_fb; + rtDW_MATLABFunction_ElacComputer_b_T sf_MATLABFunction_l0; + rtDW_MATLABFunction_ElacComputer_b_T sf_MATLABFunction_nu; + rtDW_MATLABFunction_ElacComputer_b_T sf_MATLABFunction_g4; + rtDW_MATLABFunction_ElacComputer_kz_T sf_MATLABFunction_j2; + rtDW_MATLABFunction_ElacComputer_kz_T sf_MATLABFunction_g24; + rtDW_MATLABFunction_ElacComputer_kz_T sf_MATLABFunction_lf; + rtDW_MATLABFunction_ElacComputer_kz_T sf_MATLABFunction_jl; + rtDW_MATLABFunction_ElacComputer_kz_T sf_MATLABFunction_jz; + rtDW_RateLimiter_ElacComputer_b_T sf_RateLimiter_m; + rtDW_RateLimiter_ElacComputer_b_T sf_RateLimiter_n; + rtDW_MATLABFunction_ElacComputer_kz_T sf_MATLABFunction_nb; + rtDW_MATLABFunction_ElacComputer_o_T sf_MATLABFunction_br; + rtDW_MATLABFunction_ElacComputer_o_T sf_MATLABFunction_jg; + rtDW_MATLABFunction_ElacComputer_o_T sf_MATLABFunction_mi; + rtDW_MATLABFunction_ElacComputer_kz_T sf_MATLABFunction_gfx; + rtDW_MATLABFunction_ElacComputer_kz_T sf_MATLABFunction_g2; + rtDW_MATLABFunction_ElacComputer_kz_T sf_MATLABFunction_cj; + rtDW_LagFilter_ElacComputer_T sf_LagFilter_a; + rtDW_LagFilter_ElacComputer_T sf_LagFilter; + rtDW_RateLimiter_ElacComputer_g_T sf_RateLimiter_p; + rtDW_RateLimiter_ElacComputer_g_T sf_RateLimiter_a; + rtDW_RateLimiter_ElacComputer_T sf_RateLimiter_b; + rtDW_RateLimiter_ElacComputer_T sf_RateLimiter; + }; + + struct ExternalInputs_ElacComputer_T { + elac_inputs in; + }; + + struct ExternalOutputs_ElacComputer_T { + elac_outputs out; + }; + + struct Parameters_ElacComputer_T { + real_T LagFilter_C1; + real_T LagFilter_C1_e; + real_T DiscreteDerivativeVariableTs_Gain; + real_T DiscreteTimeIntegratorVariableTsLimit_Gain; + real_T RateLimiterVariableTs2_InitialCondition; + real_T RateLimiterVariableTs3_InitialCondition; + real_T DiscreteDerivativeVariableTs_InitialCondition; + real_T BitfromLabel_bit; + real_T BitfromLabel1_bit; + real_T BitfromLabel_bit_c; + real_T BitfromLabel1_bit_j; + real_T BitfromLabel2_bit; + real_T BitfromLabel3_bit; + real_T BitfromLabel5_bit; + real_T BitfromLabel4_bit; + real_T BitfromLabel6_bit; + real_T BitfromLabel7_bit; + real_T BitfromLabel8_bit; + real_T BitfromLabel4_bit_d; + real_T BitfromLabel5_bit_e; + real_T BitfromLabel6_bit_k; + real_T BitfromLabel7_bit_h; + real_T BitfromLabel_bit_a; + real_T BitfromLabel1_bit_jr; + real_T BitfromLabel_bit_h; + real_T BitfromLabel1_bit_e; + real_T BitfromLabel2_bit_k; + real_T BitfromLabel3_bit_m; + real_T BitfromLabel_bit_h2; + real_T BitfromLabel1_bit_g; + real_T BitfromLabel2_bit_n; + real_T BitfromLabel3_bit_g; + real_T BitfromLabel4_bit_e; + real_T BitfromLabel5_bit_a; + real_T BitfromLabel_bit_e; + real_T BitfromLabel1_bit_d; + real_T BitfromLabel_bit_a2; + real_T BitfromLabel1_bit_p; + real_T BitfromLabel_bit_p; + real_T BitfromLabel1_bit_h; + real_T BitfromLabel2_bit_f; + real_T BitfromLabel3_bit_c; + real_T BitfromLabel4_bit_n; + real_T BitfromLabel5_bit_p; + real_T BitfromLabel_bit_n; + real_T BitfromLabel1_bit_h1; + real_T BitfromLabel2_bit_g; + real_T BitfromLabel3_bit_b; + real_T BitfromLabel4_bit_i; + real_T BitfromLabel5_bit_l; + real_T BitfromLabel_bit_p3; + real_T BitfromLabel2_bit_j; + real_T BitfromLabel1_bit_i; + real_T BitfromLabel3_bit_mo; + real_T BitfromLabel_bit_es; + real_T CompareToConstant_const; + real_T CompareToConstant_const_m; + real_T CompareToConstant_const_l; + real_T CompareToConstant_const_m4; + real_T CompareToConstant_const_f; + real_T CompareToConstant2_const; + real_T CompareToConstant3_const; + real_T CompareToConstant1_const; + real_T CompareToConstant1_const_d; + real_T HysteresisNode2_highTrigger; + real_T HysteresisNode1_highTrigger; + real_T HysteresisNode3_highTrigger; + real_T RateLimiterGenericVariableTs_lo; + real_T RateLimiterGenericVariableTs1_lo; + real_T RateLimiterVariableTs2_lo; + real_T RateLimiterVariableTs3_lo; + real_T RateLimiterGenericVariableTs_lo_k; + real_T RateLimiterGenericVariableTs1_lo_c; + real_T HysteresisNode2_lowTrigger; + real_T HysteresisNode1_lowTrigger; + real_T HysteresisNode3_lowTrigger; + real_T ConfirmNode_timeDelay; + real_T ConfirmNode2_timeDelay; + real_T ConfirmNode1_timeDelay; + real_T ConfirmNode_timeDelay_n; + real_T ConfirmNode1_timeDelay_h; + real_T ConfirmNode2_timeDelay_k; + real_T ConfirmNode1_timeDelay_a; + real_T ConfirmNode_timeDelay_a; + real_T ConfirmNode_timeDelay_d; + real_T ConfirmNode_timeDelay_p; + real_T RateLimiterGenericVariableTs_up; + real_T RateLimiterGenericVariableTs1_up; + real_T RateLimiterVariableTs2_up; + real_T RateLimiterVariableTs3_up; + real_T RateLimiterGenericVariableTs_up_b; + real_T RateLimiterGenericVariableTs1_up_g; + SignStatusMatrix EnumeratedConstant_Value; + SignStatusMatrix EnumeratedConstant1_Value; + lateral_efcs_law EnumeratedConstant2_Value; + pitch_efcs_law EnumeratedConstant_Value_i; + pitch_efcs_law EnumeratedConstant_Value_b; + real32_T CompareToConstant_const_ll; + boolean_T SRFlipFlop_initial_condition; + boolean_T ConfirmNode_isRisingEdge; + boolean_T ConfirmNode2_isRisingEdge; + boolean_T ConfirmNode1_isRisingEdge; + boolean_T ConfirmNode_isRisingEdge_k; + boolean_T ConfirmNode1_isRisingEdge_i; + boolean_T ConfirmNode2_isRisingEdge_j; + boolean_T PulseNode_isRisingEdge; + boolean_T PulseNode1_isRisingEdge; + boolean_T ConfirmNode1_isRisingEdge_k; + boolean_T ConfirmNode_isRisingEdge_j; + boolean_T ConfirmNode_isRisingEdge_o; + boolean_T PulseNode_isRisingEdge_g; + boolean_T ConfirmNode_isRisingEdge_f; + elac_outputs out_Y0; + base_elac_out_bus Constant4_Value; + base_elac_laws_outputs Constant_Value; + real_T Bias_Bias; + real_T Saturation_UpperSat; + real_T Saturation_LowerSat; + real_T Gain2_Gain; + real_T Constant2_Value; + real_T Constant1_Value; + real_T Constant4_Value_a; + real_T Constant3_Value; + real_T Gain_Gain; + real_T Constant2_Value_l; + real_T Constant3_Value_h; + real_T Gain_Gain_l; + real_T Saturation_UpperSat_g; + real_T Saturation_LowerSat_o; + real_T Constant_Value_n; + real_T Constant5_Value; + real_T Constant6_Value; + real_T Constant7_Value; + real_T Constant8_Value; + real_T Constant1_Value_d; + real_T Constant2_Value_b; + real_T Constant3_Value_f; + real_T Constant4_Value_i; + real_T Constant_Value_j; + real_T Constant_Value_p; + real_T Saturation_UpperSat_d; + real_T Saturation_LowerSat_h; + real_T Constant1_Value_p; + real_T Saturation1_UpperSat; + real_T Saturation1_LowerSat; + real_T alphamax_tableData[24]; + real_T alphamax_bp01Data[4]; + real_T alphamax_bp02Data[6]; + real_T alphaprotection_tableData[24]; + real_T alphaprotection_bp01Data[4]; + real_T alphaprotection_bp02Data[6]; + real_T Constant5_Value_k; + real_T Constant6_Value_b; + real_T Constant7_Value_g; + real_T Constant8_Value_h; + real_T Gain1_Gain; + real_T uDLookupTable_tableData[4]; + real_T uDLookupTable_bp01Data[4]; + real_T uDLookupTable1_tableData[4]; + real_T uDLookupTable1_bp01Data[4]; + real_T uDLookupTable2_tableData[4]; + real_T uDLookupTable2_bp01Data[4]; + real_T Constant_Value_c; + real_T Saturation2_UpperSat; + real_T Saturation2_LowerSat; + real_T Saturation1_UpperSat_g; + real_T Saturation1_LowerSat_n; + real_T Gain1_Gain_b; + real_T Constant_Value_a; + real_T Delay_InitialCondition; + real_T uDLookupTable_tableData_j[7]; + real_T uDLookupTable_bp01Data_h[7]; + real_T Constant_Value_b; + real_T Switch13_Threshold; + real_T Switch12_Threshold; + real32_T Gain_Gain_b; + real32_T Gain1_Gain_f; + real32_T Gain2_Gain_c; + real32_T Gain3_Gain; + real32_T Gain4_Gain; + uint32_T alphamax_maxIndex[2]; + uint32_T alphaprotection_maxIndex[2]; + boolean_T Constant1_Value_b; + boolean_T Constant_Value_ad; + boolean_T Delay_InitialCondition_c; + boolean_T Delay1_InitialCondition; + boolean_T Logic_table[16]; + boolean_T reset_Value; + boolean_T reset_Value_j; + boolean_T Constant1_Value_e; + boolean_T Constant9_Value; + boolean_T Constant10_Value; + }; + + ElacComputer(ElacComputer const&) = delete; + ElacComputer& operator= (ElacComputer const&) & = delete; + ElacComputer(ElacComputer &&) = delete; + ElacComputer& operator= (ElacComputer &&) = delete; + void setExternalInputs(const ExternalInputs_ElacComputer_T *pExternalInputs_ElacComputer_T) + { + ElacComputer_U = *pExternalInputs_ElacComputer_T; + } + + const ExternalOutputs_ElacComputer_T &getExternalOutputs() const + { + return ElacComputer_Y; + } + + void initialize(); + void step(); + static void terminate(); + ElacComputer(); + ~ElacComputer(); + private: + ExternalInputs_ElacComputer_T ElacComputer_U; + ExternalOutputs_ElacComputer_T ElacComputer_Y; + BlockIO_ElacComputer_T ElacComputer_B; + D_Work_ElacComputer_T ElacComputer_DWork; + static Parameters_ElacComputer_T ElacComputer_P; + static void ElacComputer_MATLABFunction(const base_arinc_429 *rtu_u, boolean_T *rty_y); + static void ElacComputer_MATLABFunction_j(const base_arinc_429 *rtu_u, real_T rtu_bit, uint32_T *rty_y); + static void ElacComputer_RateLimiter_Reset(rtDW_RateLimiter_ElacComputer_T *localDW); + static void ElacComputer_RateLimiter(real_T rtu_u, real_T rtu_up, real_T rtu_lo, real_T rtu_Ts, real_T rtu_init, + real_T *rty_Y, rtDW_RateLimiter_ElacComputer_T *localDW); + static void ElacComputer_RateLimiter_o_Reset(rtDW_RateLimiter_ElacComputer_g_T *localDW); + static void ElacComputer_RateLimiter_a(real_T rtu_u, real_T rtu_up, real_T rtu_lo, real_T rtu_Ts, real_T rtu_init, + boolean_T rtu_reset, real_T *rty_Y, rtDW_RateLimiter_ElacComputer_g_T *localDW); + static void ElacComputer_MATLABFunction_o(boolean_T rtu_bit1, boolean_T rtu_bit2, boolean_T rtu_bit3, boolean_T + rtu_bit4, boolean_T rtu_bit5, boolean_T rtu_bit6, real_T *rty_handleIndex); + static void ElacComputer_LagFilter_Reset(rtDW_LagFilter_ElacComputer_T *localDW); + static void ElacComputer_LagFilter(real_T rtu_U, real_T rtu_C1, real_T rtu_dt, real_T *rty_Y, + rtDW_LagFilter_ElacComputer_T *localDW); + static void ElacComputer_MATLABFunction_g5_Reset(rtDW_MATLABFunction_ElacComputer_kz_T *localDW); + static void ElacComputer_MATLABFunction_c(boolean_T rtu_u, real_T rtu_Ts, boolean_T rtu_isRisingEdge, real_T + rtu_timeDelay, boolean_T *rty_y, rtDW_MATLABFunction_ElacComputer_kz_T *localDW); + static void ElacComputer_MATLABFunction_k_Reset(rtDW_MATLABFunction_ElacComputer_o_T *localDW); + static void ElacComputer_MATLABFunction_m(real_T rtu_u, real_T rtu_highTrigger, real_T rtu_lowTrigger, boolean_T + *rty_y, rtDW_MATLABFunction_ElacComputer_o_T *localDW); + static void ElacComputer_GetIASforMach4(real_T rtu_m, real_T rtu_m_t, real_T rtu_v, real_T *rty_v_t); + static void ElacComputer_RateLimiter_d_Reset(rtDW_RateLimiter_ElacComputer_b_T *localDW); + static void ElacComputer_RateLimiter_n(real_T rtu_u, real_T rtu_up, real_T rtu_lo, real_T rtu_Ts, boolean_T rtu_reset, + real_T *rty_Y, rtDW_RateLimiter_ElacComputer_b_T *localDW); + static void ElacComputer_MATLABFunction_ax_Reset(rtDW_MATLABFunction_ElacComputer_b_T *localDW); + static void ElacComputer_MATLABFunction_g(boolean_T rtu_u, boolean_T rtu_isRisingEdge, boolean_T *rty_y, + rtDW_MATLABFunction_ElacComputer_b_T *localDW); + static void ElacComputer_MATLABFunction_cw(const boolean_T rtu_u[19], real32_T *rty_y); + static void ElacComputer_LateralLawCaptoBits(lateral_efcs_law rtu_law, boolean_T *rty_bit1, boolean_T *rty_bit2); + LateralDirectLaw LawMDLOBJ1; + LateralNormalLaw LawMDLOBJ2; + PitchAlternateLaw LawMDLOBJ3; + PitchDirectLaw LawMDLOBJ4; + PitchNormalLaw LawMDLOBJ5; +}; + +#endif + diff --git a/hdw-a339x/src/wasm/fbw_a320/src/model/ElacComputer_private.h b/hdw-a339x/src/wasm/fbw_a320/src/model/ElacComputer_private.h new file mode 100644 index 000000000..fc1e701dd --- /dev/null +++ b/hdw-a339x/src/wasm/fbw_a320/src/model/ElacComputer_private.h @@ -0,0 +1,6 @@ +#ifndef RTW_HEADER_ElacComputer_private_h_ +#define RTW_HEADER_ElacComputer_private_h_ +#include "rtwtypes.h" +#include "ElacComputer_types.h" +#endif + diff --git a/hdw-a339x/src/wasm/fbw_a320/src/model/FacComputer.cpp b/hdw-a339x/src/wasm/fbw_a320/src/model/FacComputer.cpp index 023c8b8e3..e2e5de936 100644 --- a/hdw-a339x/src/wasm/fbw_a320/src/model/FacComputer.cpp +++ b/hdw-a339x/src/wasm/fbw_a320/src/model/FacComputer.cpp @@ -528,8 +528,8 @@ void FacComputer::step() u0 = FacComputer_P.Constant_Value_n; } - rtb_beDot = (rtb_beDot * rtb_beDot * 0.6125 * 122.0 / (70000.0 * Vtas) * 3.172 * (FacComputer_P.Gain_Gain_h * u0) * - 3.1415926535897931 / 180.0 + (rtb_phi * 3.1415926535897931 / 180.0 * (9.81 / Vtas) - rtb_r * + rtb_beDot = (rtb_beDot * rtb_beDot * 0.6125 * 325.0 / (200000.0 * Vtas) * 3.172 * (FacComputer_P.Gain_Gain_h * u0) + * 3.1415926535897931 / 180.0 + (rtb_phi * 3.1415926535897931 / 180.0 * (9.81 / Vtas) - rtb_r * 3.1415926535897931 / 180.0)) * 180.0 / 3.1415926535897931; } else { rtb_beDot = 0.0; diff --git a/hdw-a339x/src/wasm/fbw_a320/src/model/LateralDirectLaw_private.h b/hdw-a339x/src/wasm/fbw_a320/src/model/LateralDirectLaw_private.h new file mode 100644 index 000000000..5d1ddf24a --- /dev/null +++ b/hdw-a339x/src/wasm/fbw_a320/src/model/LateralDirectLaw_private.h @@ -0,0 +1,6 @@ +#ifndef RTW_HEADER_LateralDirectLaw_private_h_ +#define RTW_HEADER_LateralDirectLaw_private_h_ +#include "rtwtypes.h" +#include "LateralDirectLaw_types.h" +#endif + diff --git a/hdw-a339x/src/wasm/fbw_a320/src/model/LateralNormalLaw.cpp b/hdw-a339x/src/wasm/fbw_a320/src/model/LateralNormalLaw.cpp new file mode 100644 index 000000000..c52750441 --- /dev/null +++ b/hdw-a339x/src/wasm/fbw_a320/src/model/LateralNormalLaw.cpp @@ -0,0 +1,599 @@ +#include "LateralNormalLaw.h" +#include "rtwtypes.h" +#include +#include "look1_binlxpw.h" + +const uint8_T LateralNormalLaw_IN_FlightMode{ 1U }; + +const uint8_T LateralNormalLaw_IN_GroundMode{ 2U }; + +const uint8_T LateralNormalLaw_IN_NO_ACTIVE_CHILD{ 0U }; + +LateralNormalLaw::Parameters_LateralNormalLaw_T LateralNormalLaw::LateralNormalLaw_rtP{ + + { 0.0, 120.0, 150.0, 380.0 }, + + + { 0.0, 130.0, 200.0, 250.0, 300.0 }, + + + { 0.0, 140.0, 180.0, 220.0, 250.0, 270.0, 300.0, 320.0, 400.0 }, + + + { 0.0, 100.0, 150.0, 200.0, 250.0, 300.0, 400.0 }, + + + { 0.0, 100.0, 150.0, 200.0, 250.0, 300.0, 400.0 }, + + + { 0.0, 0.06, 0.1, 0.2, 1.0 }, + + 0.2, + + 2.0, + + 1.0, + + 0.0, + + 0.0, + + 0.0, + + 0.0, + + 0.0, + + 0.0, + + -67.0, + + + { -15.0, -15.0, -15.0, -2.0 }, + + + { 3.0, 3.0, 2.5, 1.0, 1.0 }, + + + { 1.1, 1.3, 1.8, 2.0, 2.2, 2.5, 2.7, 3.2, 3.8 }, + + + { 4.5, 4.5, 4.5, 3.5, 2.0, 1.5, 1.5 }, + + + { 1.4, 1.4, 1.4, 1.2, 1.0, 0.8, 0.8 }, + + + { 1.1, 1.0, 0.6, 0.3, 0.1 }, + + 67.0, + + -2.0, + + -5.0, + + -15.0, + + -1000.0, + + -2.0, + + -50.0, + + 2.0, + + 5.0, + + 15.0, + + 0.33333333333333331, + + 2.0, + + 50.0, + + + { 20.0, 15.0, 0.0, -15.0, -20.0 }, + + + { -50.0, -40.0, 0.0, 40.0, 50.0 }, + + + { 20.0, 20.0, 15.0, 0.0, 0.0, 0.0, -15.0, -20.0, -20.0 }, + + + { -90.0, -66.0, -45.0, -33.0, 0.0, 33.0, 45.0, 66.0, 90.0 }, + + + { 20.0, 20.0, 15.0, 0.0, 0.0, 0.0, -15.0, -20.0, -20.0 }, + + + { -90.0, -71.0, -66.0, -33.0, 0.0, 33.0, 66.0, 71.0, 90.0 }, + + 1.0, + + 0.0, + + 1.0, + + 25.0, + + -25.0, + + 0.0, + + 1.0, + + 0.0, + + -25.0, + + 0.0, + + 0.0, + + 15.0, + + 15.0, + + -15.0, + + 0.0, + + 67.0, + + -67.0, + + 9.81, + + 0.017453292519943295, + + 0.017453292519943295, + + 1000.0, + + 100.0, + + 0.51444444444444448, + + 57.295779513082323, + + 25.0, + + -25.0, + + 1.0, + + 0.0, + + 25.0, + + -25.0, + + 1.0, + + 0.017453292519943295, + + 0.017453292519943295, + + 0.017453292519943295, + + 57.295779513082323, + + 25.0, + + -25.0, + + 1.0, + + 0.0, + + 1.0, + + 0.0, + + 1.0 +}; + +void LateralNormalLaw::LateralNormalLaw_RateLimiter_Reset(rtDW_RateLimiter_LateralNormalLaw_T *localDW) +{ + localDW->pY_not_empty = false; +} + +void LateralNormalLaw::LateralNormalLaw_RateLimiter(real_T rtu_u, real_T rtu_up, real_T rtu_lo, const real_T *rtu_Ts, + real_T rtu_init, real_T *rty_Y, rtDW_RateLimiter_LateralNormalLaw_T *localDW) +{ + if (!localDW->pY_not_empty) { + localDW->pY = rtu_init; + localDW->pY_not_empty = true; + } + + localDW->pY += std::fmax(std::fmin(rtu_u - localDW->pY, std::abs(rtu_up) * *rtu_Ts), -std::abs(rtu_lo) * *rtu_Ts); + *rty_Y = localDW->pY; +} + +void LateralNormalLaw::LateralNormalLaw_LagFilter_Reset(rtDW_LagFilter_LateralNormalLaw_T *localDW) +{ + localDW->pY_not_empty = false; + localDW->pU_not_empty = false; +} + +void LateralNormalLaw::LateralNormalLaw_LagFilter(real_T rtu_U, real_T rtu_C1, const real_T *rtu_dt, real_T *rty_Y, + rtDW_LagFilter_LateralNormalLaw_T *localDW) +{ + real_T ca; + real_T denom_tmp; + if ((!localDW->pY_not_empty) || (!localDW->pU_not_empty)) { + localDW->pU = rtu_U; + localDW->pU_not_empty = true; + localDW->pY = rtu_U; + localDW->pY_not_empty = true; + } + + denom_tmp = *rtu_dt * rtu_C1; + ca = denom_tmp / (denom_tmp + 2.0); + *rty_Y = (2.0 - denom_tmp) / (denom_tmp + 2.0) * localDW->pY + (rtu_U * ca + localDW->pU * ca); + localDW->pY = *rty_Y; + localDW->pU = rtu_U; +} + +void LateralNormalLaw::init(void) +{ + LateralNormalLaw_DWork.Delay_DSTATE = LateralNormalLaw_rtP.Delay_InitialCondition; + LateralNormalLaw_DWork.icLoad = true; +} + +void LateralNormalLaw::reset(void) +{ + LateralNormalLaw_DWork.Delay_DSTATE = LateralNormalLaw_rtP.Delay_InitialCondition; + LateralNormalLaw_DWork.icLoad = true; + LateralNormalLaw_DWork.is_active_c5_LateralNormalLaw = 0U; + LateralNormalLaw_DWork.is_c5_LateralNormalLaw = LateralNormalLaw_IN_NO_ACTIVE_CHILD; + LateralNormalLaw_RateLimiter_Reset(&LateralNormalLaw_DWork.sf_RateLimiter); + LateralNormalLaw_RateLimiter_Reset(&LateralNormalLaw_DWork.sf_RateLimiter_d); + LateralNormalLaw_RateLimiter_Reset(&LateralNormalLaw_DWork.sf_RateLimiter_n); + LateralNormalLaw_LagFilter_Reset(&LateralNormalLaw_DWork.sf_LagFilter); + LateralNormalLaw_LagFilter_Reset(&LateralNormalLaw_DWork.sf_LagFilter_m); + LateralNormalLaw_DWork.pY_not_empty_i = false; + LateralNormalLaw_DWork.pY_not_empty = false; + LateralNormalLaw_RateLimiter_Reset(&LateralNormalLaw_DWork.sf_RateLimiter_j); +} + +void LateralNormalLaw::step(const real_T *rtu_In_time_dt, const real_T *rtu_In_Theta_deg, const real_T *rtu_In_Phi_deg, + const real_T *rtu_In_r_deg_s, const real_T *rtu_In_pk_deg_s, const real_T *rtu_In_V_ias_kn, const real_T + *rtu_In_V_tas_kn, const real_T *rtu_In_H_radio_ft, const real_T *rtu_In_delta_xi_pos, const real_T + *rtu_In_delta_zeta_pos, const boolean_T *rtu_In_on_ground, const boolean_T *rtu_In_tracking_mode_on, const boolean_T + *rtu_In_high_aoa_prot_active, const boolean_T *rtu_In_high_speed_prot_active, const real_T *rtu_In_ap_phi_c_deg, const + real_T *rtu_In_ap_beta_c_deg, const boolean_T *rtu_In_any_ap_engaged, real_T *rty_Out_xi_deg, real_T *rty_Out_zeta_deg) +{ + static const int16_T b[4]{ 0, 120, 150, 380 }; + + static const int16_T b_0[4]{ 0, 120, 320, 400 }; + + static const int8_T c[4]{ -15, -15, -15, -2 }; + + static const int8_T c_0[4]{ 1, 2, 3, 3 }; + + real_T Vias; + real_T Vtas; + real_T r; + real_T rtb_Gain1; + real_T rtb_Gain1_c; + real_T rtb_Gain1_l; + real_T rtb_Gain_b; + real_T rtb_Sum_x0; + real_T rtb_Y_j; + real_T rtb_Y_p; + real_T rtb_beDot; + int32_T low_i; + int32_T low_ip1; + int32_T mid_i; + int32_T rtb_in_flight; + boolean_T rtb_OR; + if (LateralNormalLaw_DWork.is_active_c5_LateralNormalLaw == 0U) { + LateralNormalLaw_DWork.is_active_c5_LateralNormalLaw = 1U; + LateralNormalLaw_DWork.is_c5_LateralNormalLaw = LateralNormalLaw_IN_GroundMode; + rtb_in_flight = 0; + } else if (LateralNormalLaw_DWork.is_c5_LateralNormalLaw == LateralNormalLaw_IN_FlightMode) { + if (*rtu_In_on_ground) { + LateralNormalLaw_DWork.is_c5_LateralNormalLaw = LateralNormalLaw_IN_GroundMode; + rtb_in_flight = 0; + } else { + rtb_in_flight = 1; + } + } else if (((!*rtu_In_on_ground) && (*rtu_In_Theta_deg > 8.0)) || (*rtu_In_H_radio_ft > 400.0)) { + LateralNormalLaw_DWork.is_c5_LateralNormalLaw = LateralNormalLaw_IN_FlightMode; + rtb_in_flight = 1; + } else { + rtb_in_flight = 0; + } + + if (rtb_in_flight > LateralNormalLaw_rtP.Saturation_UpperSat_p) { + Vias = LateralNormalLaw_rtP.Saturation_UpperSat_p; + } else if (rtb_in_flight < LateralNormalLaw_rtP.Saturation_LowerSat_h) { + Vias = LateralNormalLaw_rtP.Saturation_LowerSat_h; + } else { + Vias = rtb_in_flight; + } + + LateralNormalLaw_RateLimiter(Vias, LateralNormalLaw_rtP.RateLimiterVariableTs_up, + LateralNormalLaw_rtP.RateLimiterVariableTs_lo, rtu_In_time_dt, + LateralNormalLaw_rtP.RateLimiterVariableTs_InitialCondition, &rtb_Y_p, &LateralNormalLaw_DWork.sf_RateLimiter); + rtb_Gain_b = LateralNormalLaw_rtP.Gain_Gain * *rtu_In_delta_xi_pos; + Vias = *rtu_In_V_ias_kn; + rtb_Sum_x0 = *rtu_In_delta_zeta_pos; + Vias = std::fmax(Vias, 60.0); + r = 0.0; + if (Vias <= 380.0) { + rtb_in_flight = 4; + low_i = 1; + low_ip1 = 2; + while (rtb_in_flight > low_ip1) { + mid_i = (low_i + rtb_in_flight) >> 1; + if (Vias >= b[mid_i - 1]) { + low_i = mid_i; + low_ip1 = mid_i + 1; + } else { + rtb_in_flight = mid_i; + } + } + + r = (Vias - static_cast(b[low_i - 1])) / static_cast(b[low_i] - b[low_i - 1]); + if (r == 0.0) { + r = -15.0; + } else if (r == 1.0) { + r = c[low_i]; + } else if (c[low_i] == -15) { + r = -15.0; + } else { + r = (1.0 - r) * -15.0 + r * static_cast(c[low_i]); + } + } + + Vias *= 0.5144; + LateralNormalLaw_RateLimiter(0.814 / std::sqrt(3.924E+6 / (398.12500000000006 * (Vias * Vias))) * (r * rtb_Sum_x0), + LateralNormalLaw_rtP.RateLimiterVariableTs1_up, LateralNormalLaw_rtP.RateLimiterVariableTs1_lo, rtu_In_time_dt, + LateralNormalLaw_rtP.RateLimiterVariableTs1_InitialCondition, &rtb_Y_j, &LateralNormalLaw_DWork.sf_RateLimiter_d); + r = *rtu_In_r_deg_s; + rtb_Y_j = *rtu_In_V_ias_kn; + Vtas = *rtu_In_V_tas_kn; + rtb_beDot = *rtu_In_delta_zeta_pos; + rtb_Gain1 = LateralNormalLaw_rtP.Gain1_Gain * *rtu_In_delta_xi_pos; + if (*rtu_In_high_speed_prot_active) { + Vias = look1_binlxpw(*rtu_In_Phi_deg, LateralNormalLaw_rtP.BankAngleProtection2_bp01Data, + LateralNormalLaw_rtP.BankAngleProtection2_tableData, 4U); + } else if (*rtu_In_high_aoa_prot_active) { + Vias = look1_binlxpw(*rtu_In_Phi_deg, LateralNormalLaw_rtP.BankAngleProtection_bp01Data, + LateralNormalLaw_rtP.BankAngleProtection_tableData, 8U); + } else { + Vias = look1_binlxpw(*rtu_In_Phi_deg, LateralNormalLaw_rtP.BankAngleProtection1_bp01Data, + LateralNormalLaw_rtP.BankAngleProtection1_tableData, 8U); + } + + rtb_Sum_x0 = 15.0; + rtb_Gain1_l = -15.0; + if (LateralNormalLaw_DWork.Delay_DSTATE >= 25.0) { + rtb_Gain1_l = *rtu_In_pk_deg_s; + } else if (LateralNormalLaw_DWork.Delay_DSTATE <= -25.0) { + rtb_Sum_x0 = *rtu_In_pk_deg_s; + } + + rtb_Gain1 += Vias; + if (rtb_Gain1 > LateralNormalLaw_rtP.Saturation_UpperSat_a) { + rtb_Gain1 = LateralNormalLaw_rtP.Saturation_UpperSat_a; + } else if (rtb_Gain1 < LateralNormalLaw_rtP.Saturation_LowerSat_o) { + rtb_Gain1 = LateralNormalLaw_rtP.Saturation_LowerSat_o; + } + + Vias = std::fmin(rtb_Sum_x0, std::fmax(rtb_Gain1_l, rtb_Gain1 * rtb_Y_p)) * + LateralNormalLaw_rtP.DiscreteTimeIntegratorVariableTs_Gain * *rtu_In_time_dt; + rtb_OR = ((rtb_Y_p == 0.0) || (*rtu_In_tracking_mode_on) || (*rtu_In_any_ap_engaged)); + rtb_Sum_x0 = *rtu_In_Phi_deg - Vias; + LateralNormalLaw_DWork.icLoad = (rtb_OR || LateralNormalLaw_DWork.icLoad); + if (LateralNormalLaw_DWork.icLoad) { + LateralNormalLaw_DWork.Delay_DSTATE_d = rtb_Sum_x0; + } + + LateralNormalLaw_DWork.Delay_DSTATE_d += Vias; + if (LateralNormalLaw_DWork.Delay_DSTATE_d > LateralNormalLaw_rtP.DiscreteTimeIntegratorVariableTs_UpperLimit) { + LateralNormalLaw_DWork.Delay_DSTATE_d = LateralNormalLaw_rtP.DiscreteTimeIntegratorVariableTs_UpperLimit; + } else if (LateralNormalLaw_DWork.Delay_DSTATE_d < LateralNormalLaw_rtP.DiscreteTimeIntegratorVariableTs_LowerLimit) { + LateralNormalLaw_DWork.Delay_DSTATE_d = LateralNormalLaw_rtP.DiscreteTimeIntegratorVariableTs_LowerLimit; + } + + if (LateralNormalLaw_DWork.Delay_DSTATE_d > LateralNormalLaw_rtP.Saturation_UpperSat_g) { + Vias = LateralNormalLaw_rtP.Saturation_UpperSat_g; + } else if (LateralNormalLaw_DWork.Delay_DSTATE_d < LateralNormalLaw_rtP.Saturation_LowerSat_e) { + Vias = LateralNormalLaw_rtP.Saturation_LowerSat_e; + } else { + Vias = LateralNormalLaw_DWork.Delay_DSTATE_d; + } + + LateralNormalLaw_RateLimiter(Vias, LateralNormalLaw_rtP.RateLimiterVariableTs_up_m, + LateralNormalLaw_rtP.RateLimiterVariableTs_lo_k, rtu_In_time_dt, + LateralNormalLaw_rtP.RateLimiterVariableTs_InitialCondition_m, &rtb_Sum_x0, &LateralNormalLaw_DWork.sf_RateLimiter_n); + if (*rtu_In_any_ap_engaged) { + if (*rtu_In_tracking_mode_on) { + rtb_Sum_x0 = *rtu_In_Phi_deg; + } else { + rtb_Sum_x0 = *rtu_In_ap_phi_c_deg; + } + } + + Vtas = std::fmax(Vtas * 0.5144, 60.0); + Vias = rtb_Y_j * 0.5144; + if (rtb_Y_j >= 60.0) { + rtb_beDot = (Vias * Vias * 0.6125 * 325.0 / (200000.0 * Vtas) * 3.172 * -rtb_beDot * 3.1415926535897931 / 180.0 + + (rtb_Sum_x0 * 3.1415926535897931 / 180.0 * (9.81 / Vtas) - r * 3.1415926535897931 / 180.0)) * 180.0 / + 3.1415926535897931; + } else { + rtb_beDot = 0.0; + } + + LateralNormalLaw_LagFilter(rtb_beDot, LateralNormalLaw_rtP.LagFilter_C1, rtu_In_time_dt, &r, + &LateralNormalLaw_DWork.sf_LagFilter); + Vias = look1_binlxpw(*rtu_In_V_ias_kn, LateralNormalLaw_rtP.ScheduledGain2_BreakpointsForDimension1, + LateralNormalLaw_rtP.ScheduledGain2_Table, 3U); + if (*rtu_In_any_ap_engaged) { + rtb_Y_j = *rtu_In_ap_beta_c_deg; + } else { + rtb_Y_j = *rtu_In_delta_zeta_pos * Vias; + } + + Vias = look1_binlxpw(*rtu_In_V_ias_kn, LateralNormalLaw_rtP.ScheduledGain1_BreakpointsForDimension1, + LateralNormalLaw_rtP.ScheduledGain1_Table, 4U); + LateralNormalLaw_LagFilter((rtb_Y_j - r) * Vias - rtb_beDot, LateralNormalLaw_rtP.LagFilter_C1_d, rtu_In_time_dt, &r, + &LateralNormalLaw_DWork.sf_LagFilter_m); + Vtas = look1_binlxpw(*rtu_In_V_ias_kn, LateralNormalLaw_rtP.ScheduledGain_BreakpointsForDimension1, + LateralNormalLaw_rtP.ScheduledGain_Table, 8U); + if (!LateralNormalLaw_DWork.pY_not_empty_i) { + LateralNormalLaw_DWork.pY_d = LateralNormalLaw_rtP.RateLimiterVariableTs_InitialCondition_d; + LateralNormalLaw_DWork.pY_not_empty_i = true; + } + + LateralNormalLaw_DWork.pY_d += std::fmax(std::fmin(static_cast(*rtu_In_on_ground) - + LateralNormalLaw_DWork.pY_d, std::abs(LateralNormalLaw_rtP.RateLimiterVariableTs_up_o) * *rtu_In_time_dt), -std::abs + (LateralNormalLaw_rtP.RateLimiterVariableTs_lo_l) * *rtu_In_time_dt); + if (*rtu_In_any_ap_engaged) { + if (LateralNormalLaw_DWork.pY_d > LateralNormalLaw_rtP.Saturation_UpperSat) { + Vias = LateralNormalLaw_rtP.Saturation_UpperSat; + } else if (LateralNormalLaw_DWork.pY_d < LateralNormalLaw_rtP.Saturation_LowerSat) { + Vias = LateralNormalLaw_rtP.Saturation_LowerSat; + } else { + Vias = LateralNormalLaw_DWork.pY_d; + } + + rtb_beDot = *rtu_In_ap_beta_c_deg * Vias; + rtb_Gain1 = rtb_Y_j * Vtas + r; + if (rtb_Gain1 > LateralNormalLaw_rtP.Saturation_UpperSat_f) { + rtb_Gain1 = LateralNormalLaw_rtP.Saturation_UpperSat_f; + } else if (rtb_Gain1 < LateralNormalLaw_rtP.Saturation_LowerSat_j) { + rtb_Gain1 = LateralNormalLaw_rtP.Saturation_LowerSat_j; + } + + Vias = (LateralNormalLaw_rtP.Constant_Value - Vias) * rtb_Gain1 + rtb_beDot; + } else { + Vias = LateralNormalLaw_rtP.Constant_Value_b; + } + + r = LateralNormalLaw_rtP.Gain1_Gain_l * *rtu_In_Theta_deg; + rtb_Gain1 = *rtu_In_V_tas_kn; + if (rtb_Gain1 > LateralNormalLaw_rtP.Saturation_UpperSat_e) { + rtb_Gain1 = LateralNormalLaw_rtP.Saturation_UpperSat_e; + } else if (rtb_Gain1 < LateralNormalLaw_rtP.Saturation_LowerSat_jd) { + rtb_Gain1 = LateralNormalLaw_rtP.Saturation_LowerSat_jd; + } + + r = *rtu_In_r_deg_s - std::sin(LateralNormalLaw_rtP.Gain1_Gain_f * rtb_Sum_x0) * LateralNormalLaw_rtP.Constant2_Value * + std::cos(r) / (LateralNormalLaw_rtP.Gain6_Gain * rtb_Gain1) * LateralNormalLaw_rtP.Gain_Gain_i; + rtb_Y_j = look1_binlxpw(*rtu_In_V_tas_kn, LateralNormalLaw_rtP.ScheduledGain_BreakpointsForDimension1_a, + LateralNormalLaw_rtP.ScheduledGain_Table_e, 6U); + rtb_beDot = r * rtb_Y_j; + rtb_OR = !*rtu_In_on_ground; + if (!LateralNormalLaw_DWork.pY_not_empty) { + LateralNormalLaw_DWork.pY = LateralNormalLaw_rtP.RateLimiterVariableTs1_InitialCondition_m; + LateralNormalLaw_DWork.pY_not_empty = true; + } + + LateralNormalLaw_DWork.pY += std::fmax(std::fmin(static_cast(rtb_OR) - LateralNormalLaw_DWork.pY, std::abs + (LateralNormalLaw_rtP.RateLimiterVariableTs1_up_j) * *rtu_In_time_dt), -std::abs + (LateralNormalLaw_rtP.RateLimiterVariableTs1_lo_n) * *rtu_In_time_dt); + if (LateralNormalLaw_DWork.pY > LateralNormalLaw_rtP.Saturation_UpperSat_n) { + r = LateralNormalLaw_rtP.Saturation_UpperSat_n; + } else if (LateralNormalLaw_DWork.pY < LateralNormalLaw_rtP.Saturation_LowerSat_b) { + r = LateralNormalLaw_rtP.Saturation_LowerSat_b; + } else { + r = LateralNormalLaw_DWork.pY; + } + + rtb_Y_j = look1_binlxpw(*rtu_In_V_tas_kn, LateralNormalLaw_rtP.ScheduledGain1_BreakpointsForDimension1_j, + LateralNormalLaw_rtP.ScheduledGain1_Table_m, 6U); + rtb_Y_j *= *rtu_In_r_deg_s; + if (rtb_beDot > LateralNormalLaw_rtP.Saturation1_UpperSat) { + rtb_beDot = LateralNormalLaw_rtP.Saturation1_UpperSat; + } else if (rtb_beDot < LateralNormalLaw_rtP.Saturation1_LowerSat) { + rtb_beDot = LateralNormalLaw_rtP.Saturation1_LowerSat; + } + + if (rtb_Y_j > LateralNormalLaw_rtP.Saturation2_UpperSat) { + rtb_Y_j = LateralNormalLaw_rtP.Saturation2_UpperSat; + } else if (rtb_Y_j < LateralNormalLaw_rtP.Saturation2_LowerSat) { + rtb_Y_j = LateralNormalLaw_rtP.Saturation2_LowerSat; + } + + *rty_Out_zeta_deg = ((LateralNormalLaw_rtP.Constant_Value_k - r) * rtb_Y_j + rtb_beDot * r) + Vias; + Vias = std::fmax(*rtu_In_V_ias_kn, 80.0) * 0.5144; + rtb_Y_j = Vias * Vias * 0.6125; + rtb_beDot = rtb_Y_j * 325.0 * 32.0 * -0.3272492347489368 / 1.0E+6; + Vtas = 0.0; + if ((*rtu_In_V_ias_kn <= 400.0) && (*rtu_In_V_ias_kn >= 0.0)) { + rtb_in_flight = 4; + low_i = 0; + low_ip1 = 2; + while (rtb_in_flight > low_ip1) { + mid_i = ((low_i + rtb_in_flight) + 1) >> 1; + if (*rtu_In_V_ias_kn >= b_0[mid_i - 1]) { + low_i = mid_i - 1; + low_ip1 = mid_i + 1; + } else { + rtb_in_flight = mid_i; + } + } + + r = (*rtu_In_V_ias_kn - static_cast(b_0[low_i])) / static_cast(b_0[low_i + 1] - b_0[low_i]); + if (r == 0.0) { + Vtas = c_0[low_i]; + } else if (r == 1.0) { + Vtas = c_0[low_i + 1]; + } else if (c_0[low_i + 1] == c_0[low_i]) { + Vtas = c_0[low_i]; + } else { + Vtas = (1.0 - r) * static_cast(c_0[low_i]) + static_cast(c_0[low_i + 1]) * r; + } + } + + rtb_Gain1 = -(Vtas * Vtas) / rtb_beDot; + rtb_Gain1_l = LateralNormalLaw_rtP.Gain1_Gain_b * *rtu_In_Phi_deg; + rtb_Gain1_c = LateralNormalLaw_rtP.Gain1_Gain_c * *rtu_In_pk_deg_s; + r = look1_binlxpw(*rtu_In_time_dt, LateralNormalLaw_rtP.ScheduledGain_BreakpointsForDimension1_j, + LateralNormalLaw_rtP.ScheduledGain_Table_i, 4U); + LateralNormalLaw_DWork.Delay_DSTATE = ((-(rtb_Y_j / Vias * 325.0 * 1024.0 * -0.75 / 1.0E+6 + 1.414 * Vtas) / rtb_beDot + * rtb_Gain1_c + rtb_Gain1 * rtb_Gain1_l) + LateralNormalLaw_rtP.Gain1_Gain_n * rtb_Sum_x0 * -rtb_Gain1) * r * + LateralNormalLaw_rtP.Gain_Gain_p; + if (rtb_Y_p > LateralNormalLaw_rtP.Saturation1_UpperSat_e) { + rtb_Y_p = LateralNormalLaw_rtP.Saturation1_UpperSat_e; + } else if (rtb_Y_p < LateralNormalLaw_rtP.Saturation1_LowerSat_l) { + rtb_Y_p = LateralNormalLaw_rtP.Saturation1_LowerSat_l; + } + + if (rtb_Y_p > LateralNormalLaw_rtP.Saturation_UpperSat_l) { + rtb_Y_j = LateralNormalLaw_rtP.Saturation_UpperSat_l; + } else if (rtb_Y_p < LateralNormalLaw_rtP.Saturation_LowerSat_og) { + rtb_Y_j = LateralNormalLaw_rtP.Saturation_LowerSat_og; + } else { + rtb_Y_j = rtb_Y_p; + } + + if (LateralNormalLaw_DWork.Delay_DSTATE > LateralNormalLaw_rtP.Limiterxi_UpperSat) { + Vias = LateralNormalLaw_rtP.Limiterxi_UpperSat; + } else if (LateralNormalLaw_DWork.Delay_DSTATE < LateralNormalLaw_rtP.Limiterxi_LowerSat) { + Vias = LateralNormalLaw_rtP.Limiterxi_LowerSat; + } else { + Vias = LateralNormalLaw_DWork.Delay_DSTATE; + } + + LateralNormalLaw_RateLimiter(Vias * rtb_Y_j + (LateralNormalLaw_rtP.Constant_Value_l1 - rtb_Y_j) * rtb_Gain_b, + LateralNormalLaw_rtP.RateLimiterVariableTs_up_d, LateralNormalLaw_rtP.RateLimiterVariableTs_lo_b, rtu_In_time_dt, + LateralNormalLaw_rtP.RateLimiterVariableTs_InitialCondition_k, rty_Out_xi_deg, + &LateralNormalLaw_DWork.sf_RateLimiter_j); + LateralNormalLaw_DWork.icLoad = false; +} + +LateralNormalLaw::LateralNormalLaw(): + LateralNormalLaw_DWork() +{ +} + +LateralNormalLaw::~LateralNormalLaw() +{ +} diff --git a/hdw-a339x/src/wasm/fbw_a320/src/model/LateralNormalLaw.h b/hdw-a339x/src/wasm/fbw_a320/src/model/LateralNormalLaw.h new file mode 100644 index 000000000..9a9fb3087 --- /dev/null +++ b/hdw-a339x/src/wasm/fbw_a320/src/model/LateralNormalLaw.h @@ -0,0 +1,155 @@ +#ifndef RTW_HEADER_LateralNormalLaw_h_ +#define RTW_HEADER_LateralNormalLaw_h_ +#include "rtwtypes.h" +#include "LateralNormalLaw_types.h" +#include + +class LateralNormalLaw final +{ + public: + struct rtDW_RateLimiter_LateralNormalLaw_T { + real_T pY; + boolean_T pY_not_empty; + }; + + struct rtDW_LagFilter_LateralNormalLaw_T { + real_T pY; + real_T pU; + boolean_T pY_not_empty; + boolean_T pU_not_empty; + }; + + struct D_Work_LateralNormalLaw_T { + real_T Delay_DSTATE; + real_T Delay_DSTATE_d; + real_T pY; + real_T pY_d; + uint8_T is_active_c5_LateralNormalLaw; + uint8_T is_c5_LateralNormalLaw; + boolean_T icLoad; + boolean_T pY_not_empty; + boolean_T pY_not_empty_i; + rtDW_RateLimiter_LateralNormalLaw_T sf_RateLimiter_j; + rtDW_LagFilter_LateralNormalLaw_T sf_LagFilter_m; + rtDW_RateLimiter_LateralNormalLaw_T sf_RateLimiter_d; + rtDW_RateLimiter_LateralNormalLaw_T sf_RateLimiter_n; + rtDW_LagFilter_LateralNormalLaw_T sf_LagFilter; + rtDW_RateLimiter_LateralNormalLaw_T sf_RateLimiter; + }; + + struct Parameters_LateralNormalLaw_T { + real_T ScheduledGain2_BreakpointsForDimension1[4]; + real_T ScheduledGain1_BreakpointsForDimension1[5]; + real_T ScheduledGain_BreakpointsForDimension1[9]; + real_T ScheduledGain_BreakpointsForDimension1_a[7]; + real_T ScheduledGain1_BreakpointsForDimension1_j[7]; + real_T ScheduledGain_BreakpointsForDimension1_j[5]; + real_T LagFilter_C1; + real_T LagFilter_C1_d; + real_T DiscreteTimeIntegratorVariableTs_Gain; + real_T RateLimiterVariableTs_InitialCondition; + real_T RateLimiterVariableTs1_InitialCondition; + real_T RateLimiterVariableTs_InitialCondition_m; + real_T RateLimiterVariableTs_InitialCondition_d; + real_T RateLimiterVariableTs1_InitialCondition_m; + real_T RateLimiterVariableTs_InitialCondition_k; + real_T DiscreteTimeIntegratorVariableTs_LowerLimit; + real_T ScheduledGain2_Table[4]; + real_T ScheduledGain1_Table[5]; + real_T ScheduledGain_Table[9]; + real_T ScheduledGain_Table_e[7]; + real_T ScheduledGain1_Table_m[7]; + real_T ScheduledGain_Table_i[5]; + real_T DiscreteTimeIntegratorVariableTs_UpperLimit; + real_T RateLimiterVariableTs_lo; + real_T RateLimiterVariableTs1_lo; + real_T RateLimiterVariableTs_lo_k; + real_T RateLimiterVariableTs_lo_l; + real_T RateLimiterVariableTs1_lo_n; + real_T RateLimiterVariableTs_lo_b; + real_T RateLimiterVariableTs_up; + real_T RateLimiterVariableTs1_up; + real_T RateLimiterVariableTs_up_m; + real_T RateLimiterVariableTs_up_o; + real_T RateLimiterVariableTs1_up_j; + real_T RateLimiterVariableTs_up_d; + real_T BankAngleProtection2_tableData[5]; + real_T BankAngleProtection2_bp01Data[5]; + real_T BankAngleProtection_tableData[9]; + real_T BankAngleProtection_bp01Data[9]; + real_T BankAngleProtection1_tableData[9]; + real_T BankAngleProtection1_bp01Data[9]; + real_T Saturation_UpperSat; + real_T Saturation_LowerSat; + real_T Constant_Value; + real_T Saturation_UpperSat_f; + real_T Saturation_LowerSat_j; + real_T Constant_Value_b; + real_T Saturation_UpperSat_p; + real_T Saturation_LowerSat_h; + real_T Gain_Gain; + real_T Constant_Value_l; + real_T Constant_Value_m; + real_T Gain1_Gain; + real_T Saturation_UpperSat_a; + real_T Saturation_LowerSat_o; + real_T Delay_InitialCondition; + real_T Saturation_UpperSat_g; + real_T Saturation_LowerSat_e; + real_T Constant2_Value; + real_T Gain1_Gain_f; + real_T Gain1_Gain_l; + real_T Saturation_UpperSat_e; + real_T Saturation_LowerSat_jd; + real_T Gain6_Gain; + real_T Gain_Gain_i; + real_T Saturation1_UpperSat; + real_T Saturation1_LowerSat; + real_T Saturation_UpperSat_n; + real_T Saturation_LowerSat_b; + real_T Saturation2_UpperSat; + real_T Saturation2_LowerSat; + real_T Constant_Value_k; + real_T Gain1_Gain_n; + real_T Gain1_Gain_b; + real_T Gain1_Gain_c; + real_T Gain_Gain_p; + real_T Limiterxi_UpperSat; + real_T Limiterxi_LowerSat; + real_T Saturation1_UpperSat_e; + real_T Saturation1_LowerSat_l; + real_T Saturation_UpperSat_l; + real_T Saturation_LowerSat_og; + real_T Constant_Value_l1; + }; + + void init(); + LateralNormalLaw(LateralNormalLaw const&) = delete; + LateralNormalLaw& operator= (LateralNormalLaw const&) & = delete; + LateralNormalLaw(LateralNormalLaw &&) = delete; + LateralNormalLaw& operator= (LateralNormalLaw &&) = delete; + void step(const real_T *rtu_In_time_dt, const real_T *rtu_In_Theta_deg, const real_T *rtu_In_Phi_deg, const real_T + *rtu_In_r_deg_s, const real_T *rtu_In_pk_deg_s, const real_T *rtu_In_V_ias_kn, const real_T *rtu_In_V_tas_kn, + const real_T *rtu_In_H_radio_ft, const real_T *rtu_In_delta_xi_pos, const real_T *rtu_In_delta_zeta_pos, + const boolean_T *rtu_In_on_ground, const boolean_T *rtu_In_tracking_mode_on, const boolean_T + *rtu_In_high_aoa_prot_active, const boolean_T *rtu_In_high_speed_prot_active, const real_T + *rtu_In_ap_phi_c_deg, const real_T *rtu_In_ap_beta_c_deg, const boolean_T *rtu_In_any_ap_engaged, real_T + *rty_Out_xi_deg, real_T *rty_Out_zeta_deg); + void reset(); + LateralNormalLaw(); + ~LateralNormalLaw(); + private: + D_Work_LateralNormalLaw_T LateralNormalLaw_DWork; + static Parameters_LateralNormalLaw_T LateralNormalLaw_rtP; + static void LateralNormalLaw_RateLimiter_Reset(rtDW_RateLimiter_LateralNormalLaw_T *localDW); + static void LateralNormalLaw_RateLimiter(real_T rtu_u, real_T rtu_up, real_T rtu_lo, const real_T *rtu_Ts, real_T + rtu_init, real_T *rty_Y, rtDW_RateLimiter_LateralNormalLaw_T *localDW); + static void LateralNormalLaw_LagFilter_Reset(rtDW_LagFilter_LateralNormalLaw_T *localDW); + static void LateralNormalLaw_LagFilter(real_T rtu_U, real_T rtu_C1, const real_T *rtu_dt, real_T *rty_Y, + rtDW_LagFilter_LateralNormalLaw_T *localDW); +}; + +extern LateralNormalLaw::Parameters_LateralNormalLaw_T LateralNormalLaw_rtP; + +#endif + diff --git a/hdw-a339x/src/wasm/fbw_a320/src/model/LateralNormalLaw_private.h b/hdw-a339x/src/wasm/fbw_a320/src/model/LateralNormalLaw_private.h new file mode 100644 index 000000000..62244ab1c --- /dev/null +++ b/hdw-a339x/src/wasm/fbw_a320/src/model/LateralNormalLaw_private.h @@ -0,0 +1,6 @@ +#ifndef RTW_HEADER_LateralNormalLaw_private_h_ +#define RTW_HEADER_LateralNormalLaw_private_h_ +#include "rtwtypes.h" +#include "LateralNormalLaw_types.h" +#endif + diff --git a/hdw-a339x/src/wasm/fbw_a320/src/model/PitchAlternateLaw.cpp b/hdw-a339x/src/wasm/fbw_a320/src/model/PitchAlternateLaw.cpp index 7c307b130..deae836f4 100644 --- a/hdw-a339x/src/wasm/fbw_a320/src/model/PitchAlternateLaw.cpp +++ b/hdw-a339x/src/wasm/fbw_a320/src/model/PitchAlternateLaw.cpp @@ -62,10 +62,10 @@ PitchAlternateLaw::Parameters_PitchAlternateLaw_T PitchAlternateLaw::PitchAltern 1.0, - 2.0, - 0.0, + 2.0, + 0.0, 0.0, @@ -136,10 +136,18 @@ PitchAlternateLaw::Parameters_PitchAlternateLaw_T PitchAlternateLaw::PitchAltern -30.0, + 1.0, + + 0.017453292519943295, + 0.017453292519943295, 0.017453292519943295, + 100.0, + + 0.1019367991845056, + { 100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0 }, @@ -154,20 +162,12 @@ PitchAlternateLaw::Parameters_PitchAlternateLaw_T PitchAlternateLaw::PitchAltern 1.0, - 0.017453292519943295, - - 100.0, - - 0.1019367991845056, - { 13.5, 13.5 }, { 0.0, 350.0 }, - 1.0, - { 0.5, 0.5 }, @@ -420,8 +420,6 @@ void PitchAlternateLaw::reset(void) PitchAlternateLaw_DWork.icLoad = true; PitchAlternateLaw_DWork.is_active_c9_PitchAlternateLaw = 0U; PitchAlternateLaw_DWork.is_c9_PitchAlternateLaw = PitchAlternateLaw_IN_NO_ACTIVE_CHILD; - PitchAlternateLaw_DWork.is_active_c8_PitchAlternateLaw = 0U; - PitchAlternateLaw_DWork.is_c8_PitchAlternateLaw = PitchAlternateLaw_IN_NO_ACTIVE_CHILD; PitchAlternateLaw_DWork.is_active_c7_PitchAlternateLaw = 0U; PitchAlternateLaw_DWork.is_c7_PitchAlternateLaw = PitchAlternateLaw_IN_NO_ACTIVE_CHILD; rtb_nz_limit_up_g = 0.0; @@ -436,6 +434,8 @@ void PitchAlternateLaw::reset(void) PitchAlternateLaw_LagFilter_Reset(&PitchAlternateLaw_DWork.sf_LagFilter_g); PitchAlternateLaw_WashoutFilter_Reset(&PitchAlternateLaw_DWork.sf_WashoutFilter_d); PitchAlternateLaw_RateLimiter_Reset(&PitchAlternateLaw_DWork.sf_RateLimiter_b); + PitchAlternateLaw_DWork.is_active_c8_PitchAlternateLaw = 0U; + PitchAlternateLaw_DWork.is_c8_PitchAlternateLaw = PitchAlternateLaw_IN_NO_ACTIVE_CHILD; } void PitchAlternateLaw::step(const real_T *rtu_In_time_dt, const real_T *rtu_In_nz_g, const real_T *rtu_In_Theta_deg, @@ -457,21 +457,21 @@ void PitchAlternateLaw::step(const real_T *rtu_In_time_dt, const real_T *rtu_In_ real_T rtb_Divide_d; real_T rtb_Divide_i; real_T rtb_Divide_n; + real_T rtb_Gain; real_T rtb_Gain1; - real_T rtb_Gain_h; - real_T rtb_Gain_j; + real_T rtb_Gain5; + real_T rtb_Gain_b; real_T rtb_Gain_m; real_T rtb_Product1_b; real_T rtb_Product1_d; real_T rtb_Product1_f; real_T rtb_Switch_c; real_T rtb_Switch_i; - real_T rtb_Y_g; real_T rtb_eta_trim_deg_rate_limit_lo_deg_s; real_T rtb_eta_trim_deg_rate_limit_up_deg_s; real_T y; real_T y_0; - int32_T rtb_TmpSignalConversionAtSFunct; + int32_T tmp; boolean_T rtb_NOT; boolean_T rtb_eta_trim_deg_should_freeze; if (PitchAlternateLaw_DWork.is_active_c9_PitchAlternateLaw == 0U) { @@ -494,39 +494,24 @@ void PitchAlternateLaw::step(const real_T *rtu_In_time_dt, const real_T *rtu_In_ rtb_eta_trim_deg_should_freeze = false; } - if (PitchAlternateLaw_DWork.is_active_c8_PitchAlternateLaw == 0U) { - PitchAlternateLaw_DWork.is_active_c8_PitchAlternateLaw = 1U; - PitchAlternateLaw_DWork.is_c8_PitchAlternateLaw = PitchAlternateLaw_IN_manual; - } else { - switch (PitchAlternateLaw_DWork.is_c8_PitchAlternateLaw) { - case PitchAlternateLaw_IN_automatic: - if (*rtu_In_in_flight == 0.0) { - PitchAlternateLaw_DWork.is_c8_PitchAlternateLaw = PitchAlternateLaw_IN_reset; - } else if (*rtu_In_tracking_mode_on) { - PitchAlternateLaw_DWork.is_c8_PitchAlternateLaw = PitchAlternateLaw_IN_tracking; - } - break; - - case PitchAlternateLaw_IN_manual: - if (*rtu_In_in_flight != 0.0) { - PitchAlternateLaw_DWork.is_c8_PitchAlternateLaw = PitchAlternateLaw_IN_automatic; - } - break; - - case PitchAlternateLaw_IN_reset: - if ((*rtu_In_in_flight == 0.0) && (*rtu_In_eta_trim_deg == 0.0)) { - PitchAlternateLaw_DWork.is_c8_PitchAlternateLaw = PitchAlternateLaw_IN_manual; - } - break; - - default: - if (!*rtu_In_tracking_mode_on) { - PitchAlternateLaw_DWork.is_c8_PitchAlternateLaw = PitchAlternateLaw_IN_automatic; - } - break; - } + rtb_Gain = PitchAlternateLaw_rtP.DiscreteDerivativeVariableTs1_Gain * *rtu_In_qk_deg_s; + rtb_Divide = (rtb_Gain - PitchAlternateLaw_DWork.Delay_DSTATE) / *rtu_In_time_dt; + rtb_Gain1 = PitchAlternateLaw_rtP.Gain1_Gain_c * *rtu_In_Theta_deg; + rtb_Cos = std::cos(rtb_Gain1); + rtb_Gain1 = PitchAlternateLaw_rtP.Gain1_Gain_l * *rtu_In_Phi_deg; + rtb_Divide1_e = rtb_Cos / std::cos(rtb_Gain1); + rtb_Gain1 = PitchAlternateLaw_rtP.Gain1_Gain_o * *rtu_In_qk_deg_s; + rtb_Gain_m = *rtu_In_nz_g - rtb_Divide1_e; + rtb_Switch_i = look1_binlxpw(*rtu_In_V_tas_kn, PitchAlternateLaw_rtP.uDLookupTable_bp01Data_o, + PitchAlternateLaw_rtP.uDLookupTable_tableData_e, 6U); + rtb_Switch_c = *rtu_In_V_tas_kn; + if (rtb_Switch_c > PitchAlternateLaw_rtP.Saturation3_UpperSat) { + rtb_Switch_c = PitchAlternateLaw_rtP.Saturation3_UpperSat; + } else if (rtb_Switch_c < PitchAlternateLaw_rtP.Saturation3_LowerSat) { + rtb_Switch_c = PitchAlternateLaw_rtP.Saturation3_LowerSat; } + rtb_Gain5 = PitchAlternateLaw_rtP.Gain5_Gain * rtb_Switch_c; if (PitchAlternateLaw_DWork.is_active_c7_PitchAlternateLaw == 0U) { PitchAlternateLaw_DWork.is_active_c7_PitchAlternateLaw = 1U; PitchAlternateLaw_DWork.is_c7_PitchAlternateLaw = PitchAlternateLaw_IN_ground; @@ -603,37 +588,20 @@ void PitchAlternateLaw::step(const real_T *rtu_In_time_dt, const real_T *rtu_In_ PitchAlternateLaw_RateLimiter(rtb_nz_limit_up_g, PitchAlternateLaw_rtP.RateLimiterVariableTs2_up, PitchAlternateLaw_rtP.RateLimiterVariableTs2_lo, rtu_In_time_dt, - PitchAlternateLaw_rtP.RateLimiterVariableTs2_InitialCondition, &rtb_Y_g, &PitchAlternateLaw_DWork.sf_RateLimiter); - rtb_Gain1 = PitchAlternateLaw_rtP.Gain1_Gain_c * *rtu_In_Theta_deg; - rtb_Cos = std::cos(rtb_Gain1); - rtb_Gain1 = PitchAlternateLaw_rtP.Gain1_Gain_l * *rtu_In_Phi_deg; - rtb_Divide1_e = rtb_Cos / std::cos(rtb_Gain1); - rtb_Switch_i = look1_binlxpw(*rtu_In_V_tas_kn, PitchAlternateLaw_rtP.uDLookupTable_bp01Data_o, - PitchAlternateLaw_rtP.uDLookupTable_tableData_e, 6U); - rtb_Product1_d = *rtu_In_V_tas_kn; - rtb_Gain1 = PitchAlternateLaw_rtP.Gain1_Gain_o * *rtu_In_qk_deg_s; - rtb_Gain_m = *rtu_In_nz_g - rtb_Divide1_e; - if (rtb_Product1_d > PitchAlternateLaw_rtP.Saturation3_UpperSat) { - rtb_Product1_d = PitchAlternateLaw_rtP.Saturation3_UpperSat; - } else if (rtb_Product1_d < PitchAlternateLaw_rtP.Saturation3_LowerSat) { - rtb_Product1_d = PitchAlternateLaw_rtP.Saturation3_LowerSat; - } - + PitchAlternateLaw_rtP.RateLimiterVariableTs2_InitialCondition, &rtb_Switch_c, + &PitchAlternateLaw_DWork.sf_RateLimiter); rtb_Switch_c = (PitchAlternateLaw_rtP.Gain_Gain_a * PitchAlternateLaw_rtP.Vm_currentms_Value * rtb_Gain1 + rtb_Gain_m) - - (rtb_Switch_i / (PitchAlternateLaw_rtP.Gain5_Gain * rtb_Product1_d) + PitchAlternateLaw_rtP.Bias_Bias) * (rtb_Y_g - - rtb_Divide1_e); + - (rtb_Switch_i / rtb_Gain5 + PitchAlternateLaw_rtP.Bias_Bias) * (rtb_Switch_c - rtb_Divide1_e); rtb_Switch_i = look1_binlxpw(*rtu_In_V_tas_kn, PitchAlternateLaw_rtP.PLUT_bp01Data, PitchAlternateLaw_rtP.PLUT_tableData, 1U); rtb_Product1_f = rtb_Switch_c * rtb_Switch_i; - rtb_Y_g = PitchAlternateLaw_rtP.DiscreteDerivativeVariableTs1_Gain * *rtu_In_qk_deg_s; - rtb_Divide = (rtb_Y_g - PitchAlternateLaw_DWork.Delay_DSTATE) / *rtu_In_time_dt; rtb_Switch_i = look1_binlxpw(*rtu_In_V_tas_kn, PitchAlternateLaw_rtP.DLUT_bp01Data, PitchAlternateLaw_rtP.DLUT_tableData, 1U); rtb_Gain1 = rtb_Switch_c * rtb_Switch_i * PitchAlternateLaw_rtP.DiscreteDerivativeVariableTs_Gain; rtb_Divide_c = (rtb_Gain1 - PitchAlternateLaw_DWork.Delay_DSTATE_k) / *rtu_In_time_dt; - rtb_Gain_j = PitchAlternateLaw_rtP.DiscreteDerivativeVariableTs2_Gain * *rtu_In_V_tas_kn; + rtb_Gain5 = PitchAlternateLaw_rtP.DiscreteDerivativeVariableTs2_Gain * *rtu_In_V_tas_kn; rtb_Switch_c = PitchAlternateLaw_DWork.Delay_DSTATE_d; - rtb_Divide_d = (rtb_Gain_j - PitchAlternateLaw_DWork.Delay_DSTATE_d) / *rtu_In_time_dt; + rtb_Divide_d = (rtb_Gain5 - PitchAlternateLaw_DWork.Delay_DSTATE_d) / *rtu_In_time_dt; PitchAlternateLaw_LagFilter(rtb_Divide_d, PitchAlternateLaw_rtP.LagFilter_C1, rtu_In_time_dt, &rtb_Switch_c, &PitchAlternateLaw_DWork.sf_LagFilter_g3); if (rtb_Switch_c > PitchAlternateLaw_rtP.SaturationV_dot_UpperSat) { @@ -655,9 +623,9 @@ void PitchAlternateLaw::step(const real_T *rtu_In_time_dt, const real_T *rtu_In_ y = rtb_Switch_c; } - rtb_Product1_f = PitchAlternateLaw_rtP.DiscreteDerivativeVariableTs1_Gain_m * *rtu_In_qk_deg_s; - rtb_Divide_n = (rtb_Product1_f - PitchAlternateLaw_DWork.Delay_DSTATE_kd) / *rtu_In_time_dt; - rtb_Divide = PitchAlternateLaw_rtP.Gain1_Gain_e * *rtu_In_qk_deg_s; + rtb_Divide = PitchAlternateLaw_rtP.DiscreteDerivativeVariableTs1_Gain_m * *rtu_In_qk_deg_s; + rtb_Divide_n = (rtb_Divide - PitchAlternateLaw_DWork.Delay_DSTATE_kd) / *rtu_In_time_dt; + rtb_Product1_f = PitchAlternateLaw_rtP.Gain1_Gain_e * *rtu_In_qk_deg_s; rtb_Switch_c = look1_binlxpw(*rtu_In_V_tas_kn, PitchAlternateLaw_rtP.uDLookupTable_bp01Data_b, PitchAlternateLaw_rtP.uDLookupTable_tableData_h, 6U); rtb_Product1_d = *rtu_In_V_tas_kn; @@ -712,7 +680,7 @@ void PitchAlternateLaw::step(const real_T *rtu_In_time_dt, const real_T *rtu_In_ rtb_Product1_d = PitchAlternateLaw_rtP.Saturation_LowerSat_o; } - rtb_Switch_c = (PitchAlternateLaw_rtP.Gain_Gain_b * PitchAlternateLaw_rtP.Vm_currentms_Value_h * rtb_Divide + + rtb_Switch_c = (PitchAlternateLaw_rtP.Gain_Gain_b * PitchAlternateLaw_rtP.Vm_currentms_Value_h * rtb_Product1_f + rtb_Gain_m) - ((((look1_binlxpw(PitchAlternateLaw_DWork.pY, PitchAlternateLaw_rtP.Loaddemand_bp01Data, PitchAlternateLaw_rtP.Loaddemand_tableData, 2U) + rtb_Switch_i) + rtb_Switch_c) + rtb_Cos / std::cos (PitchAlternateLaw_rtP.Gain1_Gain_lm * rtb_Product1_d)) - rtb_Divide1_e) * rtb_Bias_o; @@ -723,9 +691,9 @@ void PitchAlternateLaw::step(const real_T *rtu_In_time_dt, const real_T *rtu_In_ PitchAlternateLaw_rtP.DLUT_tableData_a, 1U); rtb_Cos = rtb_Switch_c * rtb_Switch_i * PitchAlternateLaw_rtP.DiscreteDerivativeVariableTs_Gain_b; rtb_Switch_i = (rtb_Cos - PitchAlternateLaw_DWork.Delay_DSTATE_j) / *rtu_In_time_dt; - rtb_Divide = PitchAlternateLaw_rtP.DiscreteDerivativeVariableTs2_Gain_c * *rtu_In_V_tas_kn; + rtb_Product1_f = PitchAlternateLaw_rtP.DiscreteDerivativeVariableTs2_Gain_c * *rtu_In_V_tas_kn; rtb_Switch_c = PitchAlternateLaw_DWork.Delay_DSTATE_dy; - rtb_Bias_o = (rtb_Divide - PitchAlternateLaw_DWork.Delay_DSTATE_dy) / *rtu_In_time_dt; + rtb_Bias_o = (rtb_Product1_f - PitchAlternateLaw_DWork.Delay_DSTATE_dy) / *rtu_In_time_dt; PitchAlternateLaw_LagFilter(rtb_Bias_o, PitchAlternateLaw_rtP.LagFilter_C1_p, rtu_In_time_dt, &rtb_Switch_c, &PitchAlternateLaw_DWork.sf_LagFilter); if (rtb_Switch_c > PitchAlternateLaw_rtP.SaturationV_dot_UpperSat_b) { @@ -784,7 +752,7 @@ void PitchAlternateLaw::step(const real_T *rtu_In_time_dt, const real_T *rtu_In_ rtb_Switch_c = PitchAlternateLaw_rtP.SaturationV_dot_LowerSat_e; } - rtb_Gain_h = PitchAlternateLaw_rtP.Gain_Gain_k * rtb_Switch_c; + rtb_Gain_b = PitchAlternateLaw_rtP.Gain_Gain_k * rtb_Switch_c; PitchAlternateLaw_WashoutFilter(rtb_Divide_d, PitchAlternateLaw_rtP.WashoutFilter_C1_h, rtu_In_time_dt, &rtb_Switch_c, &PitchAlternateLaw_DWork.sf_WashoutFilter_d); rtb_Product1_d = PitchAlternateLaw_rtP.Gain1_Gain_p * y + rtb_Divide_c; @@ -811,7 +779,7 @@ void PitchAlternateLaw::step(const real_T *rtu_In_time_dt, const real_T *rtu_In_ rtb_Switch_c = PitchAlternateLaw_rtP.SaturationSpoilers_LowerSat_l; } - rtb_Product1_d = (((PitchAlternateLaw_rtP.Gain3_Gain_b * rtb_Divide_i + rtb_Product1_b) + rtb_Switch_i) + rtb_Gain_h) + rtb_Product1_d = (((PitchAlternateLaw_rtP.Gain3_Gain_b * rtb_Divide_i + rtb_Product1_b) + rtb_Switch_i) + rtb_Gain_b) + PitchAlternateLaw_rtP.Gain1_Gain_ov * rtb_Switch_c; if (rtb_Product1_d > PitchAlternateLaw_rtP.Saturation_UpperSat_j) { rtb_TmpSignalConversionAtSFunctionInport1[2] = PitchAlternateLaw_rtP.Saturation_UpperSat_j; @@ -825,33 +793,33 @@ void PitchAlternateLaw::step(const real_T *rtu_In_time_dt, const real_T *rtu_In_ PitchAlternateLaw_rtP.ScheduledGain_Table, 5U); if (rtb_TmpSignalConversionAtSFunctionInport1[0] < rtb_TmpSignalConversionAtSFunctionInport1[1]) { if (rtb_TmpSignalConversionAtSFunctionInport1[1] < rtb_TmpSignalConversionAtSFunctionInport1[2]) { - rtb_TmpSignalConversionAtSFunct = 1; + tmp = 1; } else if (rtb_TmpSignalConversionAtSFunctionInport1[0] < rtb_TmpSignalConversionAtSFunctionInport1[2]) { - rtb_TmpSignalConversionAtSFunct = 2; + tmp = 2; } else { - rtb_TmpSignalConversionAtSFunct = 0; + tmp = 0; } } else if (rtb_TmpSignalConversionAtSFunctionInport1[0] < rtb_TmpSignalConversionAtSFunctionInport1[2]) { - rtb_TmpSignalConversionAtSFunct = 0; + tmp = 0; } else if (rtb_TmpSignalConversionAtSFunctionInport1[1] < rtb_TmpSignalConversionAtSFunctionInport1[2]) { - rtb_TmpSignalConversionAtSFunct = 2; + tmp = 2; } else { - rtb_TmpSignalConversionAtSFunct = 1; + tmp = 1; } - rtb_Divide_c = rtb_TmpSignalConversionAtSFunctionInport1[rtb_TmpSignalConversionAtSFunct] * rtb_Switch_c; + rtb_Divide_c = rtb_TmpSignalConversionAtSFunctionInport1[tmp] * rtb_Switch_c; rtb_Switch_c = look1_binlxpw(*rtu_In_time_dt, PitchAlternateLaw_rtP.ScheduledGain_BreakpointsForDimension1_d, PitchAlternateLaw_rtP.ScheduledGain_Table_h, 4U); rtb_Switch_c = rtb_Divide_c * rtb_Switch_c * PitchAlternateLaw_rtP.DiscreteTimeIntegratorVariableTs_Gain * *rtu_In_time_dt; + rtb_NOT = (*rtu_In_in_flight == 0.0); + rtb_NOT = (rtb_NOT || (*rtu_In_tracking_mode_on)); if (*rtu_In_in_flight > PitchAlternateLaw_rtP.Switch_Threshold) { rtb_Switch_i = *rtu_In_eta_deg; } else { rtb_Switch_i = PitchAlternateLaw_rtP.Gain_Gain * *rtu_In_delta_eta_pos; } - rtb_NOT = (*rtu_In_in_flight == 0.0); - rtb_NOT = (rtb_NOT || (*rtu_In_tracking_mode_on)); PitchAlternateLaw_DWork.icLoad = (rtb_NOT || PitchAlternateLaw_DWork.icLoad); if (PitchAlternateLaw_DWork.icLoad) { PitchAlternateLaw_DWork.Delay_DSTATE_o = rtb_Switch_i - rtb_Switch_c; @@ -885,12 +853,45 @@ void PitchAlternateLaw::step(const real_T *rtu_In_time_dt, const real_T *rtu_In_ rty_Out_eta_deg, &PitchAlternateLaw_DWork.sf_RateLimiter_b); *rty_Out_eta_trim_limit_up = PitchAlternateLaw_rtP.Constant2_Value; *rty_Out_eta_trim_limit_lo = PitchAlternateLaw_rtP.Constant3_Value_j; - PitchAlternateLaw_DWork.Delay_DSTATE = rtb_Y_g; + if (PitchAlternateLaw_DWork.is_active_c8_PitchAlternateLaw == 0U) { + PitchAlternateLaw_DWork.is_active_c8_PitchAlternateLaw = 1U; + PitchAlternateLaw_DWork.is_c8_PitchAlternateLaw = PitchAlternateLaw_IN_manual; + } else { + switch (PitchAlternateLaw_DWork.is_c8_PitchAlternateLaw) { + case PitchAlternateLaw_IN_automatic: + if (*rtu_In_in_flight == 0.0) { + PitchAlternateLaw_DWork.is_c8_PitchAlternateLaw = PitchAlternateLaw_IN_reset; + } else if (*rtu_In_tracking_mode_on) { + PitchAlternateLaw_DWork.is_c8_PitchAlternateLaw = PitchAlternateLaw_IN_tracking; + } + break; + + case PitchAlternateLaw_IN_manual: + if (*rtu_In_in_flight != 0.0) { + PitchAlternateLaw_DWork.is_c8_PitchAlternateLaw = PitchAlternateLaw_IN_automatic; + } + break; + + case PitchAlternateLaw_IN_reset: + if ((*rtu_In_in_flight == 0.0) && (*rtu_In_eta_trim_deg == 0.0)) { + PitchAlternateLaw_DWork.is_c8_PitchAlternateLaw = PitchAlternateLaw_IN_manual; + } + break; + + default: + if (!*rtu_In_tracking_mode_on) { + PitchAlternateLaw_DWork.is_c8_PitchAlternateLaw = PitchAlternateLaw_IN_automatic; + } + break; + } + } + + PitchAlternateLaw_DWork.Delay_DSTATE = rtb_Gain; PitchAlternateLaw_DWork.Delay_DSTATE_k = rtb_Gain1; - PitchAlternateLaw_DWork.Delay_DSTATE_d = rtb_Gain_j; - PitchAlternateLaw_DWork.Delay_DSTATE_kd = rtb_Product1_f; + PitchAlternateLaw_DWork.Delay_DSTATE_d = rtb_Gain5; + PitchAlternateLaw_DWork.Delay_DSTATE_kd = rtb_Divide; PitchAlternateLaw_DWork.Delay_DSTATE_j = rtb_Cos; - PitchAlternateLaw_DWork.Delay_DSTATE_dy = rtb_Divide; + PitchAlternateLaw_DWork.Delay_DSTATE_dy = rtb_Product1_f; PitchAlternateLaw_DWork.Delay_DSTATE_e = rtb_Divide_n; PitchAlternateLaw_DWork.Delay_DSTATE_g = rtb_Divide1_e; PitchAlternateLaw_DWork.Delay_DSTATE_l = rtb_Gain_m; diff --git a/hdw-a339x/src/wasm/fbw_a320/src/model/PitchAlternateLaw_private.h b/hdw-a339x/src/wasm/fbw_a320/src/model/PitchAlternateLaw_private.h new file mode 100644 index 000000000..8762abc59 --- /dev/null +++ b/hdw-a339x/src/wasm/fbw_a320/src/model/PitchAlternateLaw_private.h @@ -0,0 +1,6 @@ +#ifndef RTW_HEADER_PitchAlternateLaw_private_h_ +#define RTW_HEADER_PitchAlternateLaw_private_h_ +#include "rtwtypes.h" +#include "PitchAlternateLaw_types.h" +#endif + diff --git a/hdw-a339x/src/wasm/fbw_a320/src/model/PitchDirectLaw_private.h b/hdw-a339x/src/wasm/fbw_a320/src/model/PitchDirectLaw_private.h new file mode 100644 index 000000000..0e6884acf --- /dev/null +++ b/hdw-a339x/src/wasm/fbw_a320/src/model/PitchDirectLaw_private.h @@ -0,0 +1,6 @@ +#ifndef RTW_HEADER_PitchDirectLaw_private_h_ +#define RTW_HEADER_PitchDirectLaw_private_h_ +#include "rtwtypes.h" +#include "PitchDirectLaw_types.h" +#endif + diff --git a/hdw-a339x/src/wasm/fbw_a320/src/model/PitchNormalLaw.cpp b/hdw-a339x/src/wasm/fbw_a320/src/model/PitchNormalLaw.cpp index 8a3ce3ed5..520791fa0 100644 --- a/hdw-a339x/src/wasm/fbw_a320/src/model/PitchNormalLaw.cpp +++ b/hdw-a339x/src/wasm/fbw_a320/src/model/PitchNormalLaw.cpp @@ -154,6 +154,8 @@ PitchNormalLaw::Parameters_PitchNormalLaw_T PitchNormalLaw::PitchNormalLaw_rtP{ 0.0, + 0.0, + 2.0, 0.0, @@ -220,8 +222,6 @@ PitchNormalLaw::Parameters_PitchNormalLaw_T PitchNormalLaw::PitchNormalLaw_rtP{ 0.0, - 0.0, - -30.0, -30.0, @@ -245,10 +245,10 @@ PitchNormalLaw::Parameters_PitchNormalLaw_T PitchNormalLaw::PitchNormalLaw_rtP{ { 0.0, 0.0, -30.0, -30.0 }, - { 1.0, 1.0, 1.0, 1.0, 1.0 }, + { 1.0, 1.0, 0.5, 0.3, 0.3 }, - { 1.0, 1.0, 1.0, 1.0, 1.0 }, + { 1.0, 1.0, 0.5, 0.3, 0.3 }, { 1.0, 1.0, 0.5, 0.3, 0.3 }, @@ -385,61 +385,63 @@ PitchNormalLaw::Parameters_PitchNormalLaw_T PitchNormalLaw::PitchNormalLaw_rtP{ 1.0, - 0.0, + 0.017453292519943295, - 1.0, + 0.017453292519943295, - 0.0, + 0.017453292519943295, - -30.0, + 100.0, - 0.0, + 0.1019367991845056, - 0.0, - 0.0, + { 100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0 }, - 0.0, - 0.0, + { 100.0, 150.0, 200.0, 250.0, 300.0, 350.0, 400.0 }, - 0.2, + 2000.0, - 0.2, + 100.0, + + 0.51444444444444448, 1.0, - -0.25, + -30.0, + 1.0, - { -2.0, 0.0, 1.5 }, + 0.0, + 1.0, - { -1.0, 0.0, 1.0 }, + 0.0, - 0.017453292519943295, + 0.0, - 0.017453292519943295, + 0.0, + 0.0, - { 100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0 }, + 0.0, + 0.0, - { 100.0, 150.0, 200.0, 250.0, 300.0, 350.0, 400.0 }, + 0.2, - 2000.0, + 0.2, - 100.0, + 1.0, - 0.51444444444444448, + -0.25, - 1.0, - 0.017453292519943295, + { -2.0, 0.0, 1.5 }, - 100.0, - 0.1019367991845056, + { -1.0, 0.0, 1.0 }, { 13.5, 13.5 }, @@ -447,8 +449,6 @@ PitchNormalLaw::Parameters_PitchNormalLaw_T PitchNormalLaw::PitchNormalLaw_rtP{ { 0.0, 350.0 }, - 1.0, - { 0.5, 0.5 }, @@ -920,33 +920,33 @@ void PitchNormalLaw::PitchNormalLaw_VoterAttitudeProtection(real_T rtu_input, re real_T *rty_vote) { real_T rtb_TmpSignalConversionAtSFunctionInport1[3]; - int32_T rtu_input_0; + int32_T tmp; rtb_TmpSignalConversionAtSFunctionInport1[0] = rtu_input; rtb_TmpSignalConversionAtSFunctionInport1[1] = rtu_input_l; rtb_TmpSignalConversionAtSFunctionInport1[2] = rtu_input_o; if (rtu_input < rtu_input_l) { if (rtu_input_l < rtu_input_o) { - rtu_input_0 = 1; + tmp = 1; } else if (rtu_input < rtu_input_o) { - rtu_input_0 = 2; + tmp = 2; } else { - rtu_input_0 = 0; + tmp = 0; } } else if (rtu_input < rtu_input_o) { - rtu_input_0 = 0; + tmp = 0; } else if (rtu_input_l < rtu_input_o) { - rtu_input_0 = 2; + tmp = 2; } else { - rtu_input_0 = 1; + tmp = 1; } - *rty_vote = rtb_TmpSignalConversionAtSFunctionInport1[rtu_input_0]; + *rty_vote = rtb_TmpSignalConversionAtSFunctionInport1[tmp]; } void PitchNormalLaw::init(void) { - PitchNormalLaw_DWork.Delay_DSTATE = PitchNormalLaw_rtP.RateLimiterDynamicVariableTs_InitialCondition; - PitchNormalLaw_DWork.Delay_DSTATE_h = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs1_InitialCondition; + PitchNormalLaw_DWork.Delay_DSTATE = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs1_InitialCondition; + PitchNormalLaw_DWork.Delay_DSTATE_e = PitchNormalLaw_rtP.RateLimiterDynamicVariableTs_InitialCondition; PitchNormalLaw_DWork.Delay_DSTATE_n = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs_InitialCondition; PitchNormalLaw_DWork.Delay_DSTATE_c = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs2_InitialCondition; PitchNormalLaw_DWork.Delay_DSTATE_l = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs1_InitialCondition_l; @@ -958,7 +958,7 @@ void PitchNormalLaw::init(void) PitchNormalLaw_DWork.Delay_DSTATE_j = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs2_InitialCondition_m; PitchNormalLaw_DWork.Delay_DSTATE_ca = PitchNormalLaw_rtP.Delay_InitialCondition_e; PitchNormalLaw_DWork.Delay1_DSTATE_i = PitchNormalLaw_rtP.Delay1_InitialCondition_g; - PitchNormalLaw_DWork.Delay_DSTATE_e = PitchNormalLaw_rtP.RateLimiterVariableTs5_InitialCondition; + PitchNormalLaw_DWork.Delay_DSTATE_e1 = PitchNormalLaw_rtP.RateLimiterVariableTs5_InitialCondition; PitchNormalLaw_DWork.Delay_DSTATE_kd = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs1_InitialCondition_j; PitchNormalLaw_DWork.Delay_DSTATE_b = PitchNormalLaw_rtP.RateLimiterVariableTs3_InitialCondition_e; PitchNormalLaw_DWork.Delay_DSTATE_ku = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs2_InitialCondition_f; @@ -973,7 +973,7 @@ void PitchNormalLaw::init(void) PitchNormalLaw_DWork.Delay_DSTATE_e5 = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs1_InitialCondition_f; PitchNormalLaw_DWork.Delay_DSTATE_gz = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs_InitialCondition_g; PitchNormalLaw_DWork.Delay_DSTATE_lf = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs2_InitialCondition_c; - PitchNormalLaw_DWork.Delay_DSTATE_ho = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs1_InitialCondition_g; + PitchNormalLaw_DWork.Delay_DSTATE_h = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs1_InitialCondition_g; PitchNormalLaw_DWork.Delay_DSTATE_ds = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs_InitialCondition_h; PitchNormalLaw_DWork.Delay_DSTATE_jt = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs2_InitialCondition_a; PitchNormalLaw_DWork.icLoad = true; @@ -987,8 +987,8 @@ void PitchNormalLaw::reset(void) real_T rtb_nz_limit_up_g; real_T rtb_nz_limit_lo_g; real_T rtb_in_flare; - PitchNormalLaw_DWork.Delay_DSTATE = PitchNormalLaw_rtP.RateLimiterDynamicVariableTs_InitialCondition; - PitchNormalLaw_DWork.Delay_DSTATE_h = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs1_InitialCondition; + PitchNormalLaw_DWork.Delay_DSTATE = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs1_InitialCondition; + PitchNormalLaw_DWork.Delay_DSTATE_e = PitchNormalLaw_rtP.RateLimiterDynamicVariableTs_InitialCondition; PitchNormalLaw_DWork.Delay_DSTATE_n = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs_InitialCondition; PitchNormalLaw_DWork.Delay_DSTATE_c = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs2_InitialCondition; PitchNormalLaw_DWork.Delay_DSTATE_l = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs1_InitialCondition_l; @@ -1000,7 +1000,7 @@ void PitchNormalLaw::reset(void) PitchNormalLaw_DWork.Delay_DSTATE_j = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs2_InitialCondition_m; PitchNormalLaw_DWork.Delay_DSTATE_ca = PitchNormalLaw_rtP.Delay_InitialCondition_e; PitchNormalLaw_DWork.Delay1_DSTATE_i = PitchNormalLaw_rtP.Delay1_InitialCondition_g; - PitchNormalLaw_DWork.Delay_DSTATE_e = PitchNormalLaw_rtP.RateLimiterVariableTs5_InitialCondition; + PitchNormalLaw_DWork.Delay_DSTATE_e1 = PitchNormalLaw_rtP.RateLimiterVariableTs5_InitialCondition; PitchNormalLaw_DWork.Delay_DSTATE_kd = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs1_InitialCondition_j; PitchNormalLaw_DWork.Delay_DSTATE_b = PitchNormalLaw_rtP.RateLimiterVariableTs3_InitialCondition_e; PitchNormalLaw_DWork.Delay_DSTATE_ku = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs2_InitialCondition_f; @@ -1015,7 +1015,7 @@ void PitchNormalLaw::reset(void) PitchNormalLaw_DWork.Delay_DSTATE_e5 = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs1_InitialCondition_f; PitchNormalLaw_DWork.Delay_DSTATE_gz = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs_InitialCondition_g; PitchNormalLaw_DWork.Delay_DSTATE_lf = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs2_InitialCondition_c; - PitchNormalLaw_DWork.Delay_DSTATE_ho = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs1_InitialCondition_g; + PitchNormalLaw_DWork.Delay_DSTATE_h = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs1_InitialCondition_g; PitchNormalLaw_DWork.Delay_DSTATE_ds = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs_InitialCondition_h; PitchNormalLaw_DWork.Delay_DSTATE_jt = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs2_InitialCondition_a; PitchNormalLaw_DWork.icLoad = true; @@ -1085,6 +1085,7 @@ void PitchNormalLaw::step(const real_T *rtu_In_time_dt, const real_T *rtu_In_nz_ real_T rtb_Bias_o; real_T rtb_Cos; real_T rtb_Divide; + real_T rtb_Divide1_e; real_T rtb_Divide_a0; real_T rtb_Divide_an; real_T rtb_Divide_b4; @@ -1094,18 +1095,15 @@ void PitchNormalLaw::step(const real_T *rtu_In_time_dt, const real_T *rtu_In_nz_ real_T rtb_Divide_m; real_T rtb_Divide_o; real_T rtb_Divide_o2; - real_T rtb_Gain; + real_T rtb_Gain1; real_T rtb_Gain1_e; - real_T rtb_Gain1_ft; + real_T rtb_Gain1_g; + real_T rtb_Gain1_h; real_T rtb_Gain5_gq; - real_T rtb_Gain_bs; - real_T rtb_Gain_f; - real_T rtb_Gain_g; - real_T rtb_Gain_ll; - real_T rtb_Gain_ot; - real_T rtb_Gain_px; - real_T rtb_Loaddemand2; - real_T rtb_Loaddemand2_l; + real_T rtb_Gain_ci; + real_T rtb_Gain_ka; + real_T rtb_Gain_ma; + real_T rtb_Gain_mi; real_T rtb_ManualSwitch; real_T rtb_Product1_ck; real_T rtb_Product1_dm; @@ -1113,15 +1111,17 @@ void PitchNormalLaw::step(const real_T *rtu_In_time_dt, const real_T *rtu_In_nz_ real_T rtb_Product_kz; real_T rtb_Product_n3; real_T rtb_Saturation3; + real_T rtb_Saturation3_i; real_T rtb_Sum10; real_T rtb_Sum1_d4; real_T rtb_Sum_j4; - real_T rtb_Y_aj; - real_T rtb_Y_am; real_T rtb_Y_b; - real_T rtb_Y_cm; - real_T rtb_Y_ll; - real_T rtb_Y_n; + real_T rtb_Y_bc; + real_T rtb_Y_i; + real_T rtb_Y_l; + real_T rtb_Y_o; + real_T rtb_Y_o2; + real_T rtb_Y_o4; real_T rtb_Y_p; real_T rtb_alpha_err_gain; real_T rtb_eta_trim_deg_rate_limit_lo_deg_s; @@ -1169,12 +1169,12 @@ void PitchNormalLaw::step(const real_T *rtu_In_time_dt, const real_T *rtu_In_nz_ case PitchNormalLaw_IN_Flare_Set_Rate: if (PitchNormalLaw_rtP.ManualSwitch1_CurrentSetting == 1) { - rtb_Y_b = PitchNormalLaw_rtP.Constant1_Value_k; + rtb_v_target = PitchNormalLaw_rtP.Constant1_Value_k; } else { - rtb_Y_b = PitchNormalLaw_rtP.Constant_Value_p; + rtb_v_target = PitchNormalLaw_rtP.Constant_Value_p; } - if ((*rtu_In_H_radio_ft <= 30.0) || (rtb_Y_b == 1.0)) { + if ((*rtu_In_H_radio_ft <= 30.0) || (rtb_v_target == 1.0)) { PitchNormalLaw_DWork.is_c2_PitchNormalLaw = PitchNormalLaw_IN_Flare_Reduce_Theta_c; rtb_in_flare = 1.0; PitchNormalLaw_B.flare_Theta_c_deg = -2.0; @@ -1260,6 +1260,21 @@ void PitchNormalLaw::step(const real_T *rtu_In_time_dt, const real_T *rtu_In_nz_ rtb_eta_trim_deg_should_freeze = false; } + rtb_ManualSwitch = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs1_Gain * *rtu_In_qk_deg_s; + rtb_Divide = (rtb_ManualSwitch - PitchNormalLaw_DWork.Delay_DSTATE) / *rtu_In_time_dt; + rtb_Gain1 = PitchNormalLaw_rtP.Gain1_Gain * *rtu_In_Theta_deg; + rtb_Cos = std::cos(rtb_Gain1); + rtb_Gain1 = PitchNormalLaw_rtP.Gain1_Gain_l * *rtu_In_Phi_deg; + rtb_Divide1_e = rtb_Cos / std::cos(rtb_Gain1); + rtb_Gain1_h = PitchNormalLaw_rtP.Gain1_Gain_e * *rtu_In_qk_deg_s; + rtb_Gain_mi = *rtu_In_nz_g - rtb_Divide1_e; + rtb_Y_o4 = look1_binlxpw(*rtu_In_V_tas_kn, PitchNormalLaw_rtP.uDLookupTable_bp01Data, + PitchNormalLaw_rtP.uDLookupTable_tableData, 6U); + rtb_uDLookupTable = *rtu_In_V_tas_kn; + rtb_Gain_ci = *rtu_In_V_ias_kn; + rtb_Divide_an = *rtu_In_flaps_handle_index; + rtb_Gain1_g = *rtu_In_VLS_kn; + rtb_Gain1 = PitchNormalLaw_rtP.Gain_Gain_a * *rtu_In_delta_eta_pos; if (PitchNormalLaw_DWork.is_active_c8_PitchNormalLaw == 0U) { PitchNormalLaw_DWork.is_active_c8_PitchNormalLaw = 1U; PitchNormalLaw_DWork.is_c8_PitchNormalLaw = PitchNormalLaw_IN_manual; @@ -1367,29 +1382,29 @@ void PitchNormalLaw::step(const real_T *rtu_In_time_dt, const real_T *rtu_In_nz_ } } - rtb_uDLookupTable = *rtu_In_in_flight; - if (rtb_uDLookupTable > PitchNormalLaw_rtP.Saturation_UpperSat_c) { - rtb_uDLookupTable = PitchNormalLaw_rtP.Saturation_UpperSat_c; - } else if (rtb_uDLookupTable < PitchNormalLaw_rtP.Saturation_LowerSat_n) { - rtb_uDLookupTable = PitchNormalLaw_rtP.Saturation_LowerSat_n; + rtb_v_target = *rtu_In_in_flight; + if (rtb_v_target > PitchNormalLaw_rtP.Saturation_UpperSat_c) { + rtb_v_target = PitchNormalLaw_rtP.Saturation_UpperSat_c; + } else if (rtb_v_target < PitchNormalLaw_rtP.Saturation_LowerSat_n) { + rtb_v_target = PitchNormalLaw_rtP.Saturation_LowerSat_n; } - PitchNormalLaw_RateLimiter(rtb_uDLookupTable, PitchNormalLaw_rtP.RateLimiterVariableTs_up, + PitchNormalLaw_RateLimiter(rtb_v_target, PitchNormalLaw_rtP.RateLimiterVariableTs_up, PitchNormalLaw_rtP.RateLimiterVariableTs_lo, rtu_In_time_dt, - PitchNormalLaw_rtP.RateLimiterVariableTs_InitialCondition, &rtb_Y_b, &PitchNormalLaw_DWork.sf_RateLimiter); + PitchNormalLaw_rtP.RateLimiterVariableTs_InitialCondition, &rtb_Y_o2, &PitchNormalLaw_DWork.sf_RateLimiter); if (PitchNormalLaw_DWork.is_active_c6_PitchNormalLaw == 0U) { PitchNormalLaw_DWork.is_active_c6_PitchNormalLaw = 1U; PitchNormalLaw_DWork.is_c6_PitchNormalLaw = PitchNormalLaw_IN_OFF; rtb_in_rotation = 0; } else if (PitchNormalLaw_DWork.is_c6_PitchNormalLaw == PitchNormalLaw_IN_OFF) { - if ((rtb_Y_b < 1.0) && (*rtu_In_V_tas_kn > 70.0) && ((*rtu_In_thrust_lever_1_pos >= 35.0) || + if ((rtb_Y_o2 < 1.0) && (*rtu_In_V_tas_kn > 70.0) && ((*rtu_In_thrust_lever_1_pos >= 35.0) || (*rtu_In_thrust_lever_2_pos >= 35.0))) { PitchNormalLaw_DWork.is_c6_PitchNormalLaw = PitchNormalLaw_IN_ON; rtb_in_rotation = 1; } else { rtb_in_rotation = 0; } - } else if ((rtb_Y_b == 1.0) || (*rtu_In_H_radio_ft > 400.0) || ((*rtu_In_V_tas_kn < 70.0) && + } else if ((rtb_Y_o2 == 1.0) || (*rtu_In_H_radio_ft > 400.0) || ((*rtu_In_V_tas_kn < 70.0) && ((*rtu_In_thrust_lever_1_pos < 35.0) || (*rtu_In_thrust_lever_2_pos < 35.0)))) { PitchNormalLaw_DWork.is_c6_PitchNormalLaw = PitchNormalLaw_IN_OFF; rtb_in_rotation = 0; @@ -1398,119 +1413,103 @@ void PitchNormalLaw::step(const real_T *rtu_In_time_dt, const real_T *rtu_In_nz_ } if (rtb_in_rotation > PitchNormalLaw_rtP.Saturation1_UpperSat) { - rtb_Gain = PitchNormalLaw_rtP.Saturation1_UpperSat; + rtb_v_target = PitchNormalLaw_rtP.Saturation1_UpperSat; } else if (rtb_in_rotation < PitchNormalLaw_rtP.Saturation1_LowerSat) { - rtb_Gain = PitchNormalLaw_rtP.Saturation1_LowerSat; + rtb_v_target = PitchNormalLaw_rtP.Saturation1_LowerSat; } else { - rtb_Gain = rtb_in_rotation; + rtb_v_target = rtb_in_rotation; } - PitchNormalLaw_RateLimiter(rtb_Gain, PitchNormalLaw_rtP.RateLimiterVariableTs1_up, + PitchNormalLaw_RateLimiter(rtb_v_target, PitchNormalLaw_rtP.RateLimiterVariableTs1_up, PitchNormalLaw_rtP.RateLimiterVariableTs1_lo, rtu_In_time_dt, - PitchNormalLaw_rtP.RateLimiterVariableTs1_InitialCondition, &rtb_ManualSwitch, - &PitchNormalLaw_DWork.sf_RateLimiter_p); - rtb_Gain = PitchNormalLaw_rtP.Gain_Gain_a * *rtu_In_delta_eta_pos; + PitchNormalLaw_rtP.RateLimiterVariableTs1_InitialCondition, &rtb_Y_bc, &PitchNormalLaw_DWork.sf_RateLimiter_p); PitchNormalLaw_RateLimiter(rtb_nz_limit_up_g, PitchNormalLaw_rtP.RateLimiterVariableTs2_up, PitchNormalLaw_rtP.RateLimiterVariableTs2_lo, rtu_In_time_dt, PitchNormalLaw_rtP.RateLimiterVariableTs2_InitialCondition, &rtb_Y_p, &PitchNormalLaw_DWork.sf_RateLimiter_c); PitchNormalLaw_RateLimiter(rtb_nz_limit_lo_g, PitchNormalLaw_rtP.RateLimiterVariableTs3_up, PitchNormalLaw_rtP.RateLimiterVariableTs3_lo, rtu_In_time_dt, - PitchNormalLaw_rtP.RateLimiterVariableTs3_InitialCondition, &rtb_Y_ll, &PitchNormalLaw_DWork.sf_RateLimiter_n); - rtb_Loaddemand2_l = std::abs(PitchNormalLaw_B.flare_Theta_c_rate_deg_s) * *rtu_In_time_dt; - rtb_Loaddemand2 = *rtu_In_time_dt * PitchNormalLaw_B.flare_Theta_c_rate_deg_s; - PitchNormalLaw_DWork.Delay_DSTATE += std::fmax(std::fmin(PitchNormalLaw_B.flare_Theta_c_deg - - PitchNormalLaw_DWork.Delay_DSTATE, rtb_Loaddemand2_l), rtb_Loaddemand2); - PitchNormalLaw_eta_trim_limit_lofreeze(rtu_In_eta_trim_deg, rtu_In_high_aoa_prot_active, &rtb_Y_am, + PitchNormalLaw_rtP.RateLimiterVariableTs3_InitialCondition, &rtb_Y_b, &PitchNormalLaw_DWork.sf_RateLimiter_n); + rtb_Saturation3_i = std::abs(PitchNormalLaw_B.flare_Theta_c_rate_deg_s) * *rtu_In_time_dt; + rtb_Saturation3 = *rtu_In_time_dt * PitchNormalLaw_B.flare_Theta_c_rate_deg_s; + PitchNormalLaw_DWork.Delay_DSTATE_e += std::fmax(std::fmin(PitchNormalLaw_B.flare_Theta_c_deg - + PitchNormalLaw_DWork.Delay_DSTATE_e, rtb_Saturation3_i), rtb_Saturation3); + PitchNormalLaw_eta_trim_limit_lofreeze(rtu_In_eta_trim_deg, rtu_In_high_aoa_prot_active, &rtb_Y_l, &PitchNormalLaw_DWork.sf_eta_trim_limit_lofreeze); if (*rtu_In_high_aoa_prot_active) { - *rty_Out_eta_trim_limit_lo = rtb_Y_am; + *rty_Out_eta_trim_limit_lo = rtb_Y_l; } else { *rty_Out_eta_trim_limit_lo = PitchNormalLaw_rtP.Constant3_Value; } - PitchNormalLaw_eta_trim_limit_lofreeze(rtu_In_eta_trim_deg, rtu_In_high_speed_prot_active, &rtb_Y_am, + PitchNormalLaw_eta_trim_limit_lofreeze(rtu_In_eta_trim_deg, rtu_In_high_speed_prot_active, &rtb_Y_l, &PitchNormalLaw_DWork.sf_eta_trim_limit_upfreeze); if (*rtu_In_high_speed_prot_active) { - *rty_Out_eta_trim_limit_up = rtb_Y_am; + *rty_Out_eta_trim_limit_up = rtb_Y_l; } else { *rty_Out_eta_trim_limit_up = PitchNormalLaw_rtP.Constant2_Value; } - rtb_Loaddemand2_l = *rtu_In_V_ias_kn; - rtb_Gain_ot = *rtu_In_flaps_handle_index; - rtb_Gain_px = *rtu_In_VLS_kn; - if (rtb_Gain_ot == 5.0) { + if (rtb_Divide_an == 5.0) { rtb_in_rotation = 25; } else { rtb_in_rotation = 30; } - PitchNormalLaw_RateLimiter(static_cast(rtb_in_rotation) - std::fmin(5.0, std::fmax(0.0, 5.0 - - (rtb_Loaddemand2_l - (rtb_Gain_px + 5.0)) * 0.25)), PitchNormalLaw_rtP.RateLimiterVariableTs6_up, + PitchNormalLaw_RateLimiter(static_cast(rtb_in_rotation) - std::fmin(5.0, std::fmax(0.0, 5.0 - (rtb_Gain_ci - + (rtb_Gain1_g + 5.0)) * 0.25)), PitchNormalLaw_rtP.RateLimiterVariableTs6_up, PitchNormalLaw_rtP.RateLimiterVariableTs6_lo, rtu_In_time_dt, - PitchNormalLaw_rtP.RateLimiterVariableTs6_InitialCondition, &rtb_Y_am, &PitchNormalLaw_DWork.sf_RateLimiter_o); - rtb_Loaddemand2 = PitchNormalLaw_rtP.Gain1_Gain * *rtu_In_Theta_deg; - rtb_Loaddemand2_l = PitchNormalLaw_rtP.Gain1_Gain_c * *rtu_In_Theta_deg; - rtb_Cos = std::cos(rtb_Loaddemand2_l); - rtb_Loaddemand2_l = PitchNormalLaw_rtP.Gain1_Gain_l * *rtu_In_Phi_deg; - rtb_Y_cm = rtb_Cos / std::cos(rtb_Loaddemand2_l); - rtb_Y_n = look1_binlxpw(*rtu_In_V_tas_kn, PitchNormalLaw_rtP.uDLookupTable_bp01Data, - PitchNormalLaw_rtP.uDLookupTable_tableData, 6U); - rtb_uDLookupTable = *rtu_In_V_tas_kn; - rtb_Loaddemand2_l = PitchNormalLaw_rtP.Gain1_Gain_e * *rtu_In_qk_deg_s; - rtb_Gain_g = *rtu_In_nz_g - rtb_Y_cm; - rtb_Divide_kq = PitchNormalLaw_rtP.Gain2_Gain * rtb_Y_am - rtb_Loaddemand2; + PitchNormalLaw_rtP.RateLimiterVariableTs6_InitialCondition, &rtb_Y_l, &PitchNormalLaw_DWork.sf_RateLimiter_o); + rtb_Saturation3 = PitchNormalLaw_rtP.Gain1_Gain_m * *rtu_In_Theta_deg; + rtb_v_target = PitchNormalLaw_rtP.Gain2_Gain * rtb_Y_l - rtb_Saturation3; if (rtb_uDLookupTable > PitchNormalLaw_rtP.Saturation3_UpperSat) { rtb_uDLookupTable = PitchNormalLaw_rtP.Saturation3_UpperSat; } else if (rtb_uDLookupTable < PitchNormalLaw_rtP.Saturation3_LowerSat) { rtb_uDLookupTable = PitchNormalLaw_rtP.Saturation3_LowerSat; } - if (rtb_Divide_kq > PitchNormalLaw_rtP.Saturation1_UpperSat_i) { - rtb_Divide_kq = PitchNormalLaw_rtP.Saturation1_UpperSat_i; - } else if (rtb_Divide_kq < PitchNormalLaw_rtP.Saturation1_LowerSat_h) { - rtb_Divide_kq = PitchNormalLaw_rtP.Saturation1_LowerSat_h; + if (rtb_v_target > PitchNormalLaw_rtP.Saturation1_UpperSat_i) { + rtb_v_target = PitchNormalLaw_rtP.Saturation1_UpperSat_i; + } else if (rtb_v_target < PitchNormalLaw_rtP.Saturation1_LowerSat_h) { + rtb_v_target = PitchNormalLaw_rtP.Saturation1_LowerSat_h; } - rtb_Saturation3 = (PitchNormalLaw_rtP.Gain_Gain_c * PitchNormalLaw_rtP.Vm_currentms_Value * rtb_Loaddemand2_l + - rtb_Gain_g) - (rtb_Y_n / (PitchNormalLaw_rtP.Gain5_Gain * rtb_uDLookupTable) + - PitchNormalLaw_rtP.Bias_Bias) * ((rtb_Y_cm + look1_binlxpw(rtb_Divide_kq, PitchNormalLaw_rtP.Loaddemand1_bp01Data, - PitchNormalLaw_rtP.Loaddemand1_tableData, 2U)) - rtb_Y_cm); - rtb_Y_n = look1_binlxpw(*rtu_In_V_tas_kn, PitchNormalLaw_rtP.PLUT_bp01Data, PitchNormalLaw_rtP.PLUT_tableData, 1U); - rtb_Product1_dm = rtb_Saturation3 * rtb_Y_n; - rtb_Loaddemand2_l = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs1_Gain * *rtu_In_qk_deg_s; - rtb_Divide = (rtb_Loaddemand2_l - PitchNormalLaw_DWork.Delay_DSTATE_h) / *rtu_In_time_dt; - rtb_Y_n = look1_binlxpw(*rtu_In_V_tas_kn, PitchNormalLaw_rtP.DLUT_bp01Data, PitchNormalLaw_rtP.DLUT_tableData, 1U); - rtb_Gain_px = rtb_Saturation3 * rtb_Y_n * PitchNormalLaw_rtP.DiscreteDerivativeVariableTs_Gain; - rtb_Divide_o = (rtb_Gain_px - PitchNormalLaw_DWork.Delay_DSTATE_n) / *rtu_In_time_dt; - rtb_Gain_ot = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs2_Gain * *rtu_In_V_tas_kn; - rtb_Divide_an = (rtb_Gain_ot - PitchNormalLaw_DWork.Delay_DSTATE_c) / *rtu_In_time_dt; - PitchNormalLaw_LagFilter_n(rtb_Divide_an, PitchNormalLaw_rtP.LagFilter_C1_i, rtu_In_time_dt, &rtb_Y_am, + rtb_Y_i = (PitchNormalLaw_rtP.Gain_Gain_c * PitchNormalLaw_rtP.Vm_currentms_Value * rtb_Gain1_h + rtb_Gain_mi) - + (rtb_Y_o4 / (PitchNormalLaw_rtP.Gain5_Gain * rtb_uDLookupTable) + PitchNormalLaw_rtP.Bias_Bias) * ((rtb_Divide1_e + + look1_binlxpw(rtb_v_target, PitchNormalLaw_rtP.Loaddemand1_bp01Data, PitchNormalLaw_rtP.Loaddemand1_tableData, 2U)) + - rtb_Divide1_e); + rtb_Y_o4 = look1_binlxpw(*rtu_In_V_tas_kn, PitchNormalLaw_rtP.PLUT_bp01Data, PitchNormalLaw_rtP.PLUT_tableData, 1U); + rtb_Product1_dm = rtb_Y_i * rtb_Y_o4; + rtb_Y_o4 = look1_binlxpw(*rtu_In_V_tas_kn, PitchNormalLaw_rtP.DLUT_bp01Data, PitchNormalLaw_rtP.DLUT_tableData, 1U); + rtb_Gain1_h = rtb_Y_i * rtb_Y_o4 * PitchNormalLaw_rtP.DiscreteDerivativeVariableTs_Gain; + rtb_Divide_o = (rtb_Gain1_h - PitchNormalLaw_DWork.Delay_DSTATE_n) / *rtu_In_time_dt; + rtb_Gain_ci = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs2_Gain * *rtu_In_V_tas_kn; + rtb_Divide_an = (rtb_Gain_ci - PitchNormalLaw_DWork.Delay_DSTATE_c) / *rtu_In_time_dt; + PitchNormalLaw_LagFilter_n(rtb_Divide_an, PitchNormalLaw_rtP.LagFilter_C1_i, rtu_In_time_dt, &rtb_Y_l, &PitchNormalLaw_DWork.sf_LagFilter_k); - if (rtb_Y_am > PitchNormalLaw_rtP.SaturationV_dot_UpperSat) { + if (rtb_Y_l > PitchNormalLaw_rtP.SaturationV_dot_UpperSat) { rtb_Divide_a0 = PitchNormalLaw_rtP.SaturationV_dot_UpperSat; - } else if (rtb_Y_am < PitchNormalLaw_rtP.SaturationV_dot_LowerSat) { + } else if (rtb_Y_l < PitchNormalLaw_rtP.SaturationV_dot_LowerSat) { rtb_Divide_a0 = PitchNormalLaw_rtP.SaturationV_dot_LowerSat; } else { - rtb_Divide_a0 = rtb_Y_am; + rtb_Divide_a0 = rtb_Y_l; } rtb_Gain5_gq = std::fmin(*rtu_In_spoilers_left_pos, *rtu_In_spoilers_right_pos); - PitchNormalLaw_WashoutFilter(rtb_Gain5_gq, PitchNormalLaw_rtP.WashoutFilter_C1, rtu_In_time_dt, &rtb_Y_am, + PitchNormalLaw_WashoutFilter(rtb_Gain5_gq, PitchNormalLaw_rtP.WashoutFilter_C1, rtu_In_time_dt, &rtb_Y_l, &PitchNormalLaw_DWork.sf_WashoutFilter_k); - rtb_Y_n = look1_binlxpw(*rtu_In_H_radio_ft, PitchNormalLaw_rtP.ScheduledGain_BreakpointsForDimension1, + rtb_Y_o4 = look1_binlxpw(*rtu_In_H_radio_ft, PitchNormalLaw_rtP.ScheduledGain_BreakpointsForDimension1, PitchNormalLaw_rtP.ScheduledGain_Table, 3U); - if (rtb_Y_am > PitchNormalLaw_rtP.SaturationSpoilers_UpperSat) { - rtb_Y_am = PitchNormalLaw_rtP.SaturationSpoilers_UpperSat; - } else if (rtb_Y_am < PitchNormalLaw_rtP.SaturationSpoilers_LowerSat) { - rtb_Y_am = PitchNormalLaw_rtP.SaturationSpoilers_LowerSat; + if (rtb_Y_l > PitchNormalLaw_rtP.SaturationSpoilers_UpperSat) { + rtb_Y_l = PitchNormalLaw_rtP.SaturationSpoilers_UpperSat; + } else if (rtb_Y_l < PitchNormalLaw_rtP.SaturationSpoilers_LowerSat) { + rtb_Y_l = PitchNormalLaw_rtP.SaturationSpoilers_LowerSat; } - rtb_Product_kz = rtb_Y_am * rtb_Y_n; + rtb_Product_kz = rtb_Y_l * rtb_Y_o4; rtb_Divide_an = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs1_Gain_i * *rtu_In_qk_deg_s; rtb_Divide_cq = (rtb_Divide_an - PitchNormalLaw_DWork.Delay_DSTATE_l) / *rtu_In_time_dt; - rtb_Gain1_ft = PitchNormalLaw_rtP.Gain1_Gain_o * *rtu_In_qk_deg_s; - rtb_Saturation3 = look1_binlxpw(*rtu_In_V_tas_kn, PitchNormalLaw_rtP.uDLookupTable_bp01Data_o, + rtb_Gain1_g = PitchNormalLaw_rtP.Gain1_Gain_o * *rtu_In_qk_deg_s; + rtb_Y_i = look1_binlxpw(*rtu_In_V_tas_kn, PitchNormalLaw_rtP.uDLookupTable_bp01Data_o, PitchNormalLaw_rtP.uDLookupTable_tableData_e, 6U); rtb_uDLookupTable = *rtu_In_V_tas_kn; if (rtb_uDLookupTable > PitchNormalLaw_rtP.Saturation3_UpperSat_a) { @@ -1519,75 +1518,74 @@ void PitchNormalLaw::step(const real_T *rtu_In_time_dt, const real_T *rtu_In_nz_ rtb_uDLookupTable = PitchNormalLaw_rtP.Saturation3_LowerSat_l; } - rtb_Saturation3 = (PitchNormalLaw_rtP.Gain_Gain_al * PitchNormalLaw_rtP.Vm_currentms_Value_e * rtb_Gain1_ft + - rtb_Gain_g) - (rtb_Saturation3 / (PitchNormalLaw_rtP.Gain5_Gain_d * rtb_uDLookupTable) + - PitchNormalLaw_rtP.Bias_Bias_a) * (rtb_Y_p - rtb_Y_cm); - rtb_Y_n = look1_binlxpw(*rtu_In_V_tas_kn, PitchNormalLaw_rtP.PLUT_bp01Data_b, PitchNormalLaw_rtP.PLUT_tableData_b, 1U); - rtb_Product1_ck = rtb_Saturation3 * rtb_Y_n; - rtb_Y_n = look1_binlxpw(*rtu_In_V_tas_kn, PitchNormalLaw_rtP.DLUT_bp01Data_h, PitchNormalLaw_rtP.DLUT_tableData_p, 1U); - rtb_Gain1_ft = rtb_Saturation3 * rtb_Y_n * PitchNormalLaw_rtP.DiscreteDerivativeVariableTs_Gain_j; - rtb_Divide_l = (rtb_Gain1_ft - PitchNormalLaw_DWork.Delay_DSTATE_k) / *rtu_In_time_dt; - rtb_Gain_bs = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs2_Gain_e * *rtu_In_V_tas_kn; - rtb_Divide_m = (rtb_Gain_bs - PitchNormalLaw_DWork.Delay_DSTATE_d) / *rtu_In_time_dt; - PitchNormalLaw_LagFilter_n(rtb_Divide_m, PitchNormalLaw_rtP.LagFilter_C1_p, rtu_In_time_dt, &rtb_Y_am, + rtb_Y_i = (PitchNormalLaw_rtP.Gain_Gain_al * PitchNormalLaw_rtP.Vm_currentms_Value_e * rtb_Gain1_g + rtb_Gain_mi) - + (rtb_Y_i / (PitchNormalLaw_rtP.Gain5_Gain_d * rtb_uDLookupTable) + PitchNormalLaw_rtP.Bias_Bias_a) * (rtb_Y_p - + rtb_Divide1_e); + rtb_Y_o4 = look1_binlxpw(*rtu_In_V_tas_kn, PitchNormalLaw_rtP.PLUT_bp01Data_b, PitchNormalLaw_rtP.PLUT_tableData_b, 1U); + rtb_Product1_ck = rtb_Y_i * rtb_Y_o4; + rtb_Y_o4 = look1_binlxpw(*rtu_In_V_tas_kn, PitchNormalLaw_rtP.DLUT_bp01Data_h, PitchNormalLaw_rtP.DLUT_tableData_p, 1U); + rtb_Gain1_g = rtb_Y_i * rtb_Y_o4 * PitchNormalLaw_rtP.DiscreteDerivativeVariableTs_Gain_j; + rtb_Divide_l = (rtb_Gain1_g - PitchNormalLaw_DWork.Delay_DSTATE_k) / *rtu_In_time_dt; + rtb_Saturation3_i = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs2_Gain_e * *rtu_In_V_tas_kn; + rtb_Divide_m = (rtb_Saturation3_i - PitchNormalLaw_DWork.Delay_DSTATE_d) / *rtu_In_time_dt; + PitchNormalLaw_LagFilter_n(rtb_Divide_m, PitchNormalLaw_rtP.LagFilter_C1_p, rtu_In_time_dt, &rtb_Y_l, &PitchNormalLaw_DWork.sf_LagFilter_g3); - if (rtb_Y_am > PitchNormalLaw_rtP.SaturationV_dot_UpperSat_j) { - rtb_Gain_f = PitchNormalLaw_rtP.SaturationV_dot_UpperSat_j; - } else if (rtb_Y_am < PitchNormalLaw_rtP.SaturationV_dot_LowerSat_e) { - rtb_Gain_f = PitchNormalLaw_rtP.SaturationV_dot_LowerSat_e; + if (rtb_Y_l > PitchNormalLaw_rtP.SaturationV_dot_UpperSat_j) { + rtb_Gain_ma = PitchNormalLaw_rtP.SaturationV_dot_UpperSat_j; + } else if (rtb_Y_l < PitchNormalLaw_rtP.SaturationV_dot_LowerSat_e) { + rtb_Gain_ma = PitchNormalLaw_rtP.SaturationV_dot_LowerSat_e; } else { - rtb_Gain_f = rtb_Y_am; + rtb_Gain_ma = rtb_Y_l; } - PitchNormalLaw_WashoutFilter(rtb_Gain5_gq, PitchNormalLaw_rtP.WashoutFilter_C1_n, rtu_In_time_dt, &rtb_Y_am, + PitchNormalLaw_WashoutFilter(rtb_Gain5_gq, PitchNormalLaw_rtP.WashoutFilter_C1_n, rtu_In_time_dt, &rtb_Y_l, &PitchNormalLaw_DWork.sf_WashoutFilter_c); - rtb_Y_n = look1_binlxpw(*rtu_In_H_radio_ft, PitchNormalLaw_rtP.ScheduledGain_BreakpointsForDimension1_n, + rtb_Y_o4 = look1_binlxpw(*rtu_In_H_radio_ft, PitchNormalLaw_rtP.ScheduledGain_BreakpointsForDimension1_n, PitchNormalLaw_rtP.ScheduledGain_Table_b, 3U); - if (rtb_Y_am > PitchNormalLaw_rtP.SaturationSpoilers_UpperSat_g) { - rtb_Y_am = PitchNormalLaw_rtP.SaturationSpoilers_UpperSat_g; - } else if (rtb_Y_am < PitchNormalLaw_rtP.SaturationSpoilers_LowerSat_j) { - rtb_Y_am = PitchNormalLaw_rtP.SaturationSpoilers_LowerSat_j; + if (rtb_Y_l > PitchNormalLaw_rtP.SaturationSpoilers_UpperSat_g) { + rtb_Y_l = PitchNormalLaw_rtP.SaturationSpoilers_UpperSat_g; + } else if (rtb_Y_l < PitchNormalLaw_rtP.SaturationSpoilers_LowerSat_j) { + rtb_Y_l = PitchNormalLaw_rtP.SaturationSpoilers_LowerSat_j; } - rtb_Product_n3 = rtb_Y_am * rtb_Y_n; + rtb_Product_n3 = rtb_Y_l * rtb_Y_o4; PitchNormalLaw_RateLimiter_c(rtu_In_delta_eta_pos, PitchNormalLaw_rtP.RateLimiterVariableTs2_up_m, PitchNormalLaw_rtP.RateLimiterVariableTs2_lo_k, rtu_In_time_dt, PitchNormalLaw_rtP.RateLimiterVariableTs2_InitialCondition_f, &rtb_Sum_j4, &PitchNormalLaw_DWork.sf_RateLimiter_nx); rtb_Divide_m = (*rtu_In_alpha_max - *rtu_In_alpha_prot) * rtb_Sum_j4; - PitchNormalLaw_LagFilter(rtu_In_alpha_deg, PitchNormalLaw_rtP.LagFilter1_C1, rtu_In_time_dt, &rtb_Y_am, + PitchNormalLaw_LagFilter(rtu_In_alpha_deg, PitchNormalLaw_rtP.LagFilter1_C1, rtu_In_time_dt, &rtb_Y_l, &PitchNormalLaw_DWork.sf_LagFilter_m); - rtb_Sum10 = rtb_Y_am - *rtu_In_alpha_prot; + rtb_Sum10 = rtb_Y_l - *rtu_In_alpha_prot; rtb_y = std::fmax(std::fmax(0.0, *rtu_In_Theta_deg - 22.5), std::fmax(0.0, (std::abs(*rtu_In_Phi_deg) - 3.0) / 6.0)); PitchNormalLaw_WashoutFilter(rtb_y, PitchNormalLaw_rtP.WashoutFilter_C1_b, rtu_In_time_dt, &rtb_Sum_j4, &PitchNormalLaw_DWork.sf_WashoutFilter_h); - rtb_Saturation3 = (rtb_Divide_m - rtb_Sum10) - rtb_Sum_j4; - rtb_Divide_m = PitchNormalLaw_rtP.Subsystem1_Gain * rtb_Saturation3; + rtb_Y_i = (rtb_Divide_m - rtb_Sum10) - rtb_Sum_j4; + rtb_Divide_m = PitchNormalLaw_rtP.Subsystem1_Gain * rtb_Y_i; rtb_Sum10 = (rtb_Divide_m - PitchNormalLaw_DWork.Delay_DSTATE_f) / *rtu_In_time_dt; - rtb_Y_n = *rtu_In_time_dt * PitchNormalLaw_rtP.Subsystem1_C1; - rtb_Y_aj = rtb_Y_n + PitchNormalLaw_rtP.Constant_Value_f; - PitchNormalLaw_DWork.Delay1_DSTATE = 1.0 / rtb_Y_aj * (PitchNormalLaw_rtP.Constant_Value_f - rtb_Y_n) * - PitchNormalLaw_DWork.Delay1_DSTATE + (rtb_Sum10 + PitchNormalLaw_DWork.Delay_DSTATE_g) * (rtb_Y_n / rtb_Y_aj); - rtb_alpha_err_gain = PitchNormalLaw_rtP.alpha_err_gain_Gain * rtb_Saturation3; + rtb_Y_o4 = *rtu_In_time_dt * PitchNormalLaw_rtP.Subsystem1_C1; + rtb_Y_o = rtb_Y_o4 + PitchNormalLaw_rtP.Constant_Value_f; + PitchNormalLaw_DWork.Delay1_DSTATE = 1.0 / rtb_Y_o * (PitchNormalLaw_rtP.Constant_Value_f - rtb_Y_o4) * + PitchNormalLaw_DWork.Delay1_DSTATE + (rtb_Sum10 + PitchNormalLaw_DWork.Delay_DSTATE_g) * (rtb_Y_o4 / rtb_Y_o); + rtb_alpha_err_gain = PitchNormalLaw_rtP.alpha_err_gain_Gain * rtb_Y_i; rtb_y = PitchNormalLaw_rtP.Subsystem3_Gain * *rtu_In_V_ias_kn; rtb_Divide_o2 = (rtb_y - PitchNormalLaw_DWork.Delay_DSTATE_j) / *rtu_In_time_dt; - rtb_Y_aj = *rtu_In_time_dt * PitchNormalLaw_rtP.Subsystem3_C1; - rtb_Saturation3 = rtb_Y_aj + PitchNormalLaw_rtP.Constant_Value_b; - PitchNormalLaw_DWork.Delay1_DSTATE_i = 1.0 / rtb_Saturation3 * (PitchNormalLaw_rtP.Constant_Value_b - rtb_Y_aj) * - PitchNormalLaw_DWork.Delay1_DSTATE_i + (rtb_Divide_o2 + PitchNormalLaw_DWork.Delay_DSTATE_ca) * (rtb_Y_aj / - rtb_Saturation3); + rtb_Y_o = *rtu_In_time_dt * PitchNormalLaw_rtP.Subsystem3_C1; + rtb_Y_i = rtb_Y_o + PitchNormalLaw_rtP.Constant_Value_b; + PitchNormalLaw_DWork.Delay1_DSTATE_i = 1.0 / rtb_Y_i * (PitchNormalLaw_rtP.Constant_Value_b - rtb_Y_o) * + PitchNormalLaw_DWork.Delay1_DSTATE_i + (rtb_Divide_o2 + PitchNormalLaw_DWork.Delay_DSTATE_ca) * (rtb_Y_o / rtb_Y_i); rtb_qk_gain = PitchNormalLaw_rtP.qk_gain_Gain * *rtu_In_qk_deg_s; rtb_qk_dot_gain = PitchNormalLaw_rtP.qk_dot_gain_Gain * *rtu_In_qk_dot_deg_s2; - rtb_Y_n = *rtu_In_high_aoa_prot_active; + rtb_Y_o4 = *rtu_In_high_aoa_prot_active; rtb_Sum_j4 = PitchNormalLaw_rtP.RateLimiterVariableTs5_up * *rtu_In_time_dt; - rtb_Y_n = std::fmin(rtb_Y_n - PitchNormalLaw_DWork.Delay_DSTATE_e, rtb_Sum_j4); + rtb_Y_o4 = std::fmin(rtb_Y_o4 - PitchNormalLaw_DWork.Delay_DSTATE_e1, rtb_Sum_j4); rtb_Sum_j4 = *rtu_In_time_dt * PitchNormalLaw_rtP.RateLimiterVariableTs5_lo; - PitchNormalLaw_DWork.Delay_DSTATE_e += std::fmax(rtb_Y_n, rtb_Sum_j4); - if (PitchNormalLaw_DWork.Delay_DSTATE_e > PitchNormalLaw_rtP.Saturation_UpperSat_eo) { + PitchNormalLaw_DWork.Delay_DSTATE_e1 += std::fmax(rtb_Y_o4, rtb_Sum_j4); + if (PitchNormalLaw_DWork.Delay_DSTATE_e1 > PitchNormalLaw_rtP.Saturation_UpperSat_eo) { rtb_Sum_j4 = PitchNormalLaw_rtP.Saturation_UpperSat_eo; - } else if (PitchNormalLaw_DWork.Delay_DSTATE_e < PitchNormalLaw_rtP.Saturation_LowerSat_h) { + } else if (PitchNormalLaw_DWork.Delay_DSTATE_e1 < PitchNormalLaw_rtP.Saturation_LowerSat_h) { rtb_Sum_j4 = PitchNormalLaw_rtP.Saturation_LowerSat_h; } else { - rtb_Sum_j4 = PitchNormalLaw_DWork.Delay_DSTATE_e; + rtb_Sum_j4 = PitchNormalLaw_DWork.Delay_DSTATE_e1; } rtb_uDLookupTable = (((PitchNormalLaw_rtP.precontrol_gain_Gain * PitchNormalLaw_DWork.Delay1_DSTATE + @@ -1621,79 +1619,79 @@ void PitchNormalLaw::step(const real_T *rtu_In_time_dt, const real_T *rtu_In_nz_ PitchNormalLaw_rtP.ScheduledGain_Table_j, 6U); PitchNormalLaw_RateLimiter_c(rtu_In_delta_eta_pos, PitchNormalLaw_rtP.RateLimiterVariableTs_up_n, PitchNormalLaw_rtP.RateLimiterVariableTs_lo_c, rtu_In_time_dt, - PitchNormalLaw_rtP.RateLimiterVariableTs_InitialCondition_o, &rtb_Y_am, &PitchNormalLaw_DWork.sf_RateLimiter_c2); - rtb_Y_aj = *rtu_In_delta_eta_pos - PitchNormalLaw_DWork.Delay_DSTATE_b; - rtb_Saturation3 = PitchNormalLaw_rtP.RateLimiterVariableTs3_up_i * *rtu_In_time_dt; - rtb_Y_aj = std::fmin(rtb_Y_aj, rtb_Saturation3); - rtb_Saturation3 = *rtu_In_time_dt * PitchNormalLaw_rtP.RateLimiterVariableTs3_lo_b; - PitchNormalLaw_DWork.Delay_DSTATE_b += std::fmax(rtb_Y_aj, rtb_Saturation3); + PitchNormalLaw_rtP.RateLimiterVariableTs_InitialCondition_o, &rtb_Y_l, &PitchNormalLaw_DWork.sf_RateLimiter_c2); + rtb_Y_o = *rtu_In_delta_eta_pos - PitchNormalLaw_DWork.Delay_DSTATE_b; + rtb_Y_i = PitchNormalLaw_rtP.RateLimiterVariableTs3_up_i * *rtu_In_time_dt; + rtb_Y_o = std::fmin(rtb_Y_o, rtb_Y_i); + rtb_Y_i = *rtu_In_time_dt * PitchNormalLaw_rtP.RateLimiterVariableTs3_lo_b; + PitchNormalLaw_DWork.Delay_DSTATE_b += std::fmax(rtb_Y_o, rtb_Y_i); rtb_v_target = std::fmax((*rtu_In_high_speed_prot_low_kn - *rtu_In_high_speed_prot_high_kn) * PitchNormalLaw_DWork.Delay_DSTATE_b, 0.0) + *rtu_In_high_speed_prot_low_kn; rtb_qk_gain = PitchNormalLaw_rtP.Subsystem2_Gain * rtb_v_target; rtb_qk_dot_gain = (rtb_qk_gain - PitchNormalLaw_DWork.Delay_DSTATE_ku) / *rtu_In_time_dt; rtb_Sum_j4 = *rtu_In_time_dt * PitchNormalLaw_rtP.Subsystem2_C1; - rtb_Y_aj = rtb_Sum_j4 + PitchNormalLaw_rtP.Constant_Value_ja; - PitchNormalLaw_DWork.Delay1_DSTATE_l = 1.0 / rtb_Y_aj * (PitchNormalLaw_rtP.Constant_Value_ja - rtb_Sum_j4) * + rtb_Y_o = rtb_Sum_j4 + PitchNormalLaw_rtP.Constant_Value_ja; + PitchNormalLaw_DWork.Delay1_DSTATE_l = 1.0 / rtb_Y_o * (PitchNormalLaw_rtP.Constant_Value_ja - rtb_Sum_j4) * PitchNormalLaw_DWork.Delay1_DSTATE_l + (rtb_qk_dot_gain + PitchNormalLaw_DWork.Delay_DSTATE_gl) * (rtb_Sum_j4 / - rtb_Y_aj); - rtb_Gain_ll = PitchNormalLaw_rtP.Subsystem_Gain * *rtu_In_V_ias_kn; - rtb_Divide_b4 = (rtb_Gain_ll - PitchNormalLaw_DWork.Delay_DSTATE_m) / *rtu_In_time_dt; + rtb_Y_o); + rtb_Gain_ka = PitchNormalLaw_rtP.Subsystem_Gain * *rtu_In_V_ias_kn; + rtb_Divide_b4 = (rtb_Gain_ka - PitchNormalLaw_DWork.Delay_DSTATE_m) / *rtu_In_time_dt; rtb_Sum_j4 = *rtu_In_time_dt * PitchNormalLaw_rtP.Subsystem_C1; - rtb_Y_aj = rtb_Sum_j4 + PitchNormalLaw_rtP.Constant_Value_jj; - PitchNormalLaw_DWork.Delay1_DSTATE_n = 1.0 / rtb_Y_aj * (PitchNormalLaw_rtP.Constant_Value_jj - rtb_Sum_j4) * + rtb_Y_o = rtb_Sum_j4 + PitchNormalLaw_rtP.Constant_Value_jj; + PitchNormalLaw_DWork.Delay1_DSTATE_n = 1.0 / rtb_Y_o * (PitchNormalLaw_rtP.Constant_Value_jj - rtb_Sum_j4) * PitchNormalLaw_DWork.Delay1_DSTATE_n + (rtb_Divide_b4 + PitchNormalLaw_DWork.Delay_DSTATE_k2) * (rtb_Sum_j4 / - rtb_Y_aj); + rtb_Y_o); rtb_in_rotation = *rtu_In_high_speed_prot_active; - rtb_Saturation3 = PitchNormalLaw_rtP.RateLimiterVariableTs4_up * *rtu_In_time_dt; - rtb_Y_aj = std::fmin(static_cast(rtb_in_rotation) - PitchNormalLaw_DWork.Delay_DSTATE_mz, rtb_Saturation3); - rtb_Saturation3 = *rtu_In_time_dt * PitchNormalLaw_rtP.RateLimiterVariableTs4_lo; - PitchNormalLaw_DWork.Delay_DSTATE_mz += std::fmax(rtb_Y_aj, rtb_Saturation3); + rtb_Y_i = PitchNormalLaw_rtP.RateLimiterVariableTs4_up * *rtu_In_time_dt; + rtb_Y_o = std::fmin(static_cast(rtb_in_rotation) - PitchNormalLaw_DWork.Delay_DSTATE_mz, rtb_Y_i); + rtb_Y_i = *rtu_In_time_dt * PitchNormalLaw_rtP.RateLimiterVariableTs4_lo; + PitchNormalLaw_DWork.Delay_DSTATE_mz += std::fmax(rtb_Y_o, rtb_Y_i); PitchNormalLaw_RateLimiter(rtb_in_flare, PitchNormalLaw_rtP.RateLimiterVariableTs6_up_n, PitchNormalLaw_rtP.RateLimiterVariableTs6_lo_p, rtu_In_time_dt, - PitchNormalLaw_rtP.RateLimiterVariableTs6_InitialCondition_f, &rtb_Y_n, &PitchNormalLaw_DWork.sf_RateLimiter_l); + PitchNormalLaw_rtP.RateLimiterVariableTs6_InitialCondition_f, &rtb_Y_o4, &PitchNormalLaw_DWork.sf_RateLimiter_l); if (*rtu_In_any_ap_engaged) { rtb_Sum_j4 = rtb_Y_p - *rtu_In_Theta_deg; rtb_Sum_j4 *= rtb_uDLookupTable; } else { - rtb_Sum_j4 = look1_binlxpw(rtb_Y_am, PitchNormalLaw_rtP.Loaddemand_bp01Data, PitchNormalLaw_rtP.Loaddemand_tableData, + rtb_Sum_j4 = look1_binlxpw(rtb_Y_l, PitchNormalLaw_rtP.Loaddemand_bp01Data, PitchNormalLaw_rtP.Loaddemand_tableData, 2U); - if (rtb_Y_n > PitchNormalLaw_rtP.Saturation_UpperSat) { - rtb_Y_n = PitchNormalLaw_rtP.Saturation_UpperSat; - } else if (rtb_Y_n < PitchNormalLaw_rtP.Saturation_LowerSat) { - rtb_Y_n = PitchNormalLaw_rtP.Saturation_LowerSat; + if (rtb_Y_o4 > PitchNormalLaw_rtP.Saturation_UpperSat) { + rtb_Y_o4 = PitchNormalLaw_rtP.Saturation_UpperSat; + } else if (rtb_Y_o4 < PitchNormalLaw_rtP.Saturation_LowerSat) { + rtb_Y_o4 = PitchNormalLaw_rtP.Saturation_LowerSat; } - rtb_Y_p = PitchNormalLaw_DWork.Delay_DSTATE - *rtu_In_Theta_deg; - rtb_uDLookupTable = PitchNormalLaw_rtP.Gain_Gain * rtb_Y_p; + rtb_uDLookupTable = PitchNormalLaw_DWork.Delay_DSTATE_e - *rtu_In_Theta_deg; + rtb_uDLookupTable *= PitchNormalLaw_rtP.Gain_Gain; if (rtb_uDLookupTable > PitchNormalLaw_rtP.Saturation_UpperSat_g) { rtb_uDLookupTable = PitchNormalLaw_rtP.Saturation_UpperSat_g; } else if (rtb_uDLookupTable < PitchNormalLaw_rtP.Saturation_LowerSat_d) { rtb_uDLookupTable = PitchNormalLaw_rtP.Saturation_LowerSat_d; } - rtb_Y_p = (PitchNormalLaw_rtP.Constant_Value_g - rtb_Y_n) * PitchNormalLaw_rtP.Constant_Value_l + rtb_uDLookupTable * - rtb_Y_n; + rtb_Y_p = (PitchNormalLaw_rtP.Constant_Value_g - rtb_Y_o4) * PitchNormalLaw_rtP.Constant_Value_l + rtb_uDLookupTable + * rtb_Y_o4; if (PitchNormalLaw_DWork.Delay_DSTATE_mz > PitchNormalLaw_rtP.Saturation_UpperSat_e) { - rtb_Y_am = PitchNormalLaw_rtP.Saturation_UpperSat_e; + rtb_Y_l = PitchNormalLaw_rtP.Saturation_UpperSat_e; } else if (PitchNormalLaw_DWork.Delay_DSTATE_mz < PitchNormalLaw_rtP.Saturation_LowerSat_m) { - rtb_Y_am = PitchNormalLaw_rtP.Saturation_LowerSat_m; + rtb_Y_l = PitchNormalLaw_rtP.Saturation_LowerSat_m; } else { - rtb_Y_am = PitchNormalLaw_DWork.Delay_DSTATE_mz; + rtb_Y_l = PitchNormalLaw_DWork.Delay_DSTATE_mz; } if (rtb_in_rotation > PitchNormalLaw_rtP.Switch2_Threshold) { - rtb_Y_n = PitchNormalLaw_rtP.qk_dot_gain1_Gain * *rtu_In_qk_dot_deg_s2; + rtb_Y_o4 = PitchNormalLaw_rtP.qk_dot_gain1_Gain * *rtu_In_qk_dot_deg_s2; rtb_uDLookupTable = PitchNormalLaw_rtP.qk_gain_HSP_Gain * *rtu_In_qk_deg_s; - rtb_Y_aj = rtb_v_target - *rtu_In_V_ias_kn; + rtb_Y_o = rtb_v_target - *rtu_In_V_ias_kn; rtb_uDLookupTable = ((((PitchNormalLaw_rtP.precontrol_gain_HSP_Gain * PitchNormalLaw_DWork.Delay1_DSTATE_l + - PitchNormalLaw_rtP.Gain6_Gain * rtb_Y_aj) + PitchNormalLaw_rtP.v_dot_gain_HSP_Gain * - PitchNormalLaw_DWork.Delay1_DSTATE_n) + rtb_uDLookupTable) + rtb_Y_n) * PitchNormalLaw_rtP.HSP_gain_Gain; + PitchNormalLaw_rtP.Gain6_Gain * rtb_Y_o) + PitchNormalLaw_rtP.v_dot_gain_HSP_Gain * + PitchNormalLaw_DWork.Delay1_DSTATE_n) + rtb_uDLookupTable) + rtb_Y_o4) * PitchNormalLaw_rtP.HSP_gain_Gain; if (rtb_Sum_j4 > PitchNormalLaw_rtP.Saturation8_UpperSat) { - rtb_Y_n = PitchNormalLaw_rtP.Saturation8_UpperSat; + rtb_v_target = PitchNormalLaw_rtP.Saturation8_UpperSat; } else if (rtb_Sum_j4 < PitchNormalLaw_rtP.Saturation8_LowerSat) { - rtb_Y_n = PitchNormalLaw_rtP.Saturation8_LowerSat; + rtb_v_target = PitchNormalLaw_rtP.Saturation8_LowerSat; } else { - rtb_Y_n = rtb_Sum_j4; + rtb_v_target = rtb_Sum_j4; } if (rtb_uDLookupTable > PitchNormalLaw_rtP.Saturation4_UpperSat) { @@ -1702,12 +1700,12 @@ void PitchNormalLaw::step(const real_T *rtu_In_time_dt, const real_T *rtu_In_nz_ rtb_uDLookupTable = PitchNormalLaw_rtP.Saturation4_LowerSat; } - rtb_Y_n += rtb_uDLookupTable; + rtb_Y_o4 = rtb_v_target + rtb_uDLookupTable; } else { - rtb_Y_n = PitchNormalLaw_rtP.Constant1_Value; + rtb_Y_o4 = PitchNormalLaw_rtP.Constant1_Value; } - rtb_Sum_j4 = ((PitchNormalLaw_rtP.Constant_Value_m - rtb_Y_am) * rtb_Sum_j4 + rtb_Y_n * rtb_Y_am) + rtb_Y_p; + rtb_Sum_j4 = ((PitchNormalLaw_rtP.Constant_Value_m - rtb_Y_l) * rtb_Sum_j4 + rtb_Y_o4 * rtb_Y_l) + rtb_Y_p; } rtb_uDLookupTable = *rtu_In_Phi_deg; @@ -1717,17 +1715,18 @@ void PitchNormalLaw::step(const real_T *rtu_In_time_dt, const real_T *rtu_In_nz_ rtb_uDLookupTable = PitchNormalLaw_rtP.Saturation_LowerSat_o1; } - rtb_Sum_j4 = (PitchNormalLaw_rtP.Gain_Gain_b * PitchNormalLaw_rtP.Vm_currentms_Value_h * rtb_Gain1_e + rtb_Gain_g) - - ((rtb_Cos / std::cos(PitchNormalLaw_rtP.Gain1_Gain_lm * rtb_uDLookupTable) + rtb_Sum_j4) - rtb_Y_cm) * rtb_Bias_o; - rtb_Y_aj = look1_binlxpw(*rtu_In_V_tas_kn, PitchNormalLaw_rtP.PLUT_bp01Data_f, PitchNormalLaw_rtP.PLUT_tableData_k, 1U); - rtb_Y_n = rtb_Sum_j4 * rtb_Y_aj; - rtb_Y_aj = look1_binlxpw(*rtu_In_V_tas_kn, PitchNormalLaw_rtP.DLUT_bp01Data_m, PitchNormalLaw_rtP.DLUT_tableData_a, 1U); - rtb_Cos = rtb_Sum_j4 * rtb_Y_aj * PitchNormalLaw_rtP.DiscreteDerivativeVariableTs_Gain_b; - rtb_uDLookupTable = (rtb_Cos - PitchNormalLaw_DWork.Delay_DSTATE_jh) / *rtu_In_time_dt; - rtb_Y_p = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs2_Gain_c * *rtu_In_V_tas_kn; + rtb_Sum_j4 = (PitchNormalLaw_rtP.Gain_Gain_b * PitchNormalLaw_rtP.Vm_currentms_Value_h * rtb_Gain1_e + rtb_Gain_mi) - + ((rtb_Cos / std::cos(PitchNormalLaw_rtP.Gain1_Gain_lm * rtb_uDLookupTable) + rtb_Sum_j4) - rtb_Divide1_e) * + rtb_Bias_o; + rtb_Y_o = look1_binlxpw(*rtu_In_V_tas_kn, PitchNormalLaw_rtP.PLUT_bp01Data_f, PitchNormalLaw_rtP.PLUT_tableData_k, 1U); + rtb_uDLookupTable = rtb_Sum_j4 * rtb_Y_o; + rtb_Y_o = look1_binlxpw(*rtu_In_V_tas_kn, PitchNormalLaw_rtP.DLUT_bp01Data_m, PitchNormalLaw_rtP.DLUT_tableData_a, 1U); + rtb_Cos = rtb_Sum_j4 * rtb_Y_o * PitchNormalLaw_rtP.DiscreteDerivativeVariableTs_Gain_b; + rtb_Y_p = (rtb_Cos - PitchNormalLaw_DWork.Delay_DSTATE_jh) / *rtu_In_time_dt; + rtb_Y_o4 = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs2_Gain_c * *rtu_In_V_tas_kn; rtb_Sum_j4 = PitchNormalLaw_DWork.Delay_DSTATE_dy; - rtb_Y_am = (rtb_Y_p - PitchNormalLaw_DWork.Delay_DSTATE_dy) / *rtu_In_time_dt; - PitchNormalLaw_LagFilter_n(rtb_Y_am, PitchNormalLaw_rtP.LagFilter_C1_pt, rtu_In_time_dt, &rtb_Sum_j4, + rtb_Y_l = (rtb_Y_o4 - PitchNormalLaw_DWork.Delay_DSTATE_dy) / *rtu_In_time_dt; + PitchNormalLaw_LagFilter_n(rtb_Y_l, PitchNormalLaw_rtP.LagFilter_C1_pt, rtu_In_time_dt, &rtb_Sum_j4, &PitchNormalLaw_DWork.sf_LagFilter_i); if (rtb_Sum_j4 > PitchNormalLaw_rtP.SaturationV_dot_UpperSat_b) { rtb_Sum_j4 = PitchNormalLaw_rtP.SaturationV_dot_UpperSat_b; @@ -1735,10 +1734,10 @@ void PitchNormalLaw::step(const real_T *rtu_In_time_dt, const real_T *rtu_In_nz_ rtb_Sum_j4 = PitchNormalLaw_rtP.SaturationV_dot_LowerSat_m; } - rtb_Y_am = PitchNormalLaw_rtP.Gain_Gain_f * rtb_Sum_j4; + rtb_Y_l = PitchNormalLaw_rtP.Gain_Gain_f * rtb_Sum_j4; PitchNormalLaw_WashoutFilter(rtb_Gain5_gq, PitchNormalLaw_rtP.WashoutFilter_C1_l, rtu_In_time_dt, &rtb_Sum_j4, &PitchNormalLaw_DWork.sf_WashoutFilter_l); - rtb_Y_aj = look1_binlxpw(*rtu_In_H_radio_ft, PitchNormalLaw_rtP.ScheduledGain_BreakpointsForDimension1_c, + rtb_Y_o = look1_binlxpw(*rtu_In_H_radio_ft, PitchNormalLaw_rtP.ScheduledGain_BreakpointsForDimension1_c, PitchNormalLaw_rtP.ScheduledGain_Table_g, 3U); if (rtb_Sum_j4 > PitchNormalLaw_rtP.SaturationSpoilers_UpperSat_o) { rtb_Sum_j4 = PitchNormalLaw_rtP.SaturationSpoilers_UpperSat_o; @@ -1746,32 +1745,32 @@ void PitchNormalLaw::step(const real_T *rtu_In_time_dt, const real_T *rtu_In_nz_ rtb_Sum_j4 = PitchNormalLaw_rtP.SaturationSpoilers_LowerSat_jl; } - rtb_uDLookupTable = (((PitchNormalLaw_rtP.Gain3_Gain_c * rtb_Divide_kq + rtb_Y_n) + rtb_uDLookupTable) + rtb_Y_am) + - rtb_Sum_j4 * rtb_Y_aj; - rtb_Y_n = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs1_Gain_c * *rtu_In_qk_deg_s; - rtb_Saturation3 = (rtb_Y_n - PitchNormalLaw_DWork.Delay_DSTATE_e5) / *rtu_In_time_dt; - rtb_Y_am = PitchNormalLaw_rtP.Gain1_Gain_b * *rtu_In_qk_deg_s; + rtb_uDLookupTable = (((PitchNormalLaw_rtP.Gain3_Gain_c * rtb_Divide_kq + rtb_uDLookupTable) + rtb_Y_p) + rtb_Y_l) + + rtb_Sum_j4 * rtb_Y_o; + rtb_Y_p = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs1_Gain_c * *rtu_In_qk_deg_s; + rtb_Y_i = (rtb_Y_p - PitchNormalLaw_DWork.Delay_DSTATE_e5) / *rtu_In_time_dt; + rtb_Y_l = PitchNormalLaw_rtP.Gain1_Gain_b * *rtu_In_qk_deg_s; rtb_Sum_j4 = look1_binlxpw(*rtu_In_V_tas_kn, PitchNormalLaw_rtP.uDLookupTable_bp01Data_a, PitchNormalLaw_rtP.uDLookupTable_tableData_p, 6U); - rtb_Divide_kq = *rtu_In_V_tas_kn; - if (rtb_Divide_kq > PitchNormalLaw_rtP.Saturation3_UpperSat_n) { - rtb_Divide_kq = PitchNormalLaw_rtP.Saturation3_UpperSat_n; - } else if (rtb_Divide_kq < PitchNormalLaw_rtP.Saturation3_LowerSat_a) { - rtb_Divide_kq = PitchNormalLaw_rtP.Saturation3_LowerSat_a; + rtb_v_target = *rtu_In_V_tas_kn; + if (rtb_v_target > PitchNormalLaw_rtP.Saturation3_UpperSat_n) { + rtb_v_target = PitchNormalLaw_rtP.Saturation3_UpperSat_n; + } else if (rtb_v_target < PitchNormalLaw_rtP.Saturation3_LowerSat_a) { + rtb_v_target = PitchNormalLaw_rtP.Saturation3_LowerSat_a; } - rtb_Sum_j4 = (PitchNormalLaw_rtP.Gain_Gain_p * PitchNormalLaw_rtP.Vm_currentms_Value_p * rtb_Y_am + rtb_Gain_g) - - (rtb_Sum_j4 / (PitchNormalLaw_rtP.Gain5_Gain_n * rtb_Divide_kq) + PitchNormalLaw_rtP.Bias_Bias_ai) * (rtb_Y_ll - - rtb_Y_cm); - rtb_Y_aj = look1_binlxpw(*rtu_In_V_tas_kn, PitchNormalLaw_rtP.PLUT_bp01Data_a, PitchNormalLaw_rtP.PLUT_tableData_o, 1U); - rtb_Gain1_e = rtb_Sum_j4 * rtb_Y_aj; - rtb_Y_aj = look1_binlxpw(*rtu_In_V_tas_kn, PitchNormalLaw_rtP.DLUT_bp01Data_k, PitchNormalLaw_rtP.DLUT_tableData_e, 1U); - rtb_Y_ll = rtb_Sum_j4 * rtb_Y_aj * PitchNormalLaw_rtP.DiscreteDerivativeVariableTs_Gain_p; - rtb_Bias_o = (rtb_Y_ll - PitchNormalLaw_DWork.Delay_DSTATE_gz) / *rtu_In_time_dt; - rtb_Y_am = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs2_Gain_a * *rtu_In_V_tas_kn; + rtb_Sum_j4 = (PitchNormalLaw_rtP.Gain_Gain_p * PitchNormalLaw_rtP.Vm_currentms_Value_p * rtb_Y_l + rtb_Gain_mi) - + (rtb_Sum_j4 / (PitchNormalLaw_rtP.Gain5_Gain_n * rtb_v_target) + PitchNormalLaw_rtP.Bias_Bias_ai) * (rtb_Y_b - + rtb_Divide1_e); + rtb_Y_o = look1_binlxpw(*rtu_In_V_tas_kn, PitchNormalLaw_rtP.PLUT_bp01Data_a, PitchNormalLaw_rtP.PLUT_tableData_o, 1U); + rtb_Divide_kq = rtb_Sum_j4 * rtb_Y_o; + rtb_Y_o = look1_binlxpw(*rtu_In_V_tas_kn, PitchNormalLaw_rtP.DLUT_bp01Data_k, PitchNormalLaw_rtP.DLUT_tableData_e, 1U); + rtb_Y_b = rtb_Sum_j4 * rtb_Y_o * PitchNormalLaw_rtP.DiscreteDerivativeVariableTs_Gain_p; + rtb_Gain1_e = (rtb_Y_b - PitchNormalLaw_DWork.Delay_DSTATE_gz) / *rtu_In_time_dt; + rtb_Y_l = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs2_Gain_a * *rtu_In_V_tas_kn; rtb_Sum_j4 = PitchNormalLaw_DWork.Delay_DSTATE_lf; - rtb_Y_aj = (rtb_Y_am - PitchNormalLaw_DWork.Delay_DSTATE_lf) / *rtu_In_time_dt; - PitchNormalLaw_LagFilter_n(rtb_Y_aj, PitchNormalLaw_rtP.LagFilter_C1_l, rtu_In_time_dt, &rtb_Sum_j4, + rtb_Y_o = (rtb_Y_l - PitchNormalLaw_DWork.Delay_DSTATE_lf) / *rtu_In_time_dt; + PitchNormalLaw_LagFilter_n(rtb_Y_o, PitchNormalLaw_rtP.LagFilter_C1_l, rtu_In_time_dt, &rtb_Sum_j4, &PitchNormalLaw_DWork.sf_LagFilter_g); if (rtb_Sum_j4 > PitchNormalLaw_rtP.SaturationV_dot_UpperSat_m) { rtb_Sum_j4 = PitchNormalLaw_rtP.SaturationV_dot_UpperSat_m; @@ -1779,25 +1778,25 @@ void PitchNormalLaw::step(const real_T *rtu_In_time_dt, const real_T *rtu_In_nz_ rtb_Sum_j4 = PitchNormalLaw_rtP.SaturationV_dot_LowerSat_ek; } - rtb_v_target = PitchNormalLaw_rtP.Gain_Gain_k * rtb_Sum_j4; + rtb_Bias_o = PitchNormalLaw_rtP.Gain_Gain_k * rtb_Sum_j4; PitchNormalLaw_WashoutFilter(rtb_Gain5_gq, PitchNormalLaw_rtP.WashoutFilter_C1_h, rtu_In_time_dt, &rtb_Sum_j4, &PitchNormalLaw_DWork.sf_WashoutFilter_d); - rtb_Y_aj = look1_binlxpw(*rtu_In_H_radio_ft, PitchNormalLaw_rtP.ScheduledGain_BreakpointsForDimension1_f, + rtb_Y_o = look1_binlxpw(*rtu_In_H_radio_ft, PitchNormalLaw_rtP.ScheduledGain_BreakpointsForDimension1_f, PitchNormalLaw_rtP.ScheduledGain_Table_h, 3U); - rtb_Divide_kq = (((PitchNormalLaw_rtP.Gain3_Gain_m * rtb_Divide_cq + rtb_Product1_ck) + rtb_Divide_l) + - PitchNormalLaw_rtP.Gain_Gain_j * rtb_Gain_f) + rtb_Product_n3; + rtb_v_target = (((PitchNormalLaw_rtP.Gain3_Gain_m * rtb_Divide_cq + rtb_Product1_ck) + rtb_Divide_l) + + PitchNormalLaw_rtP.Gain_Gain_j * rtb_Gain_ma) + rtb_Product_n3; if (rtb_Sum_j4 > PitchNormalLaw_rtP.SaturationSpoilers_UpperSat_h) { rtb_Sum_j4 = PitchNormalLaw_rtP.SaturationSpoilers_UpperSat_h; } else if (rtb_Sum_j4 < PitchNormalLaw_rtP.SaturationSpoilers_LowerSat_l) { rtb_Sum_j4 = PitchNormalLaw_rtP.SaturationSpoilers_LowerSat_l; } - rtb_Sum_j4 = (((PitchNormalLaw_rtP.Gain3_Gain_b * rtb_Saturation3 + rtb_Gain1_e) + rtb_Bias_o) + rtb_v_target) + - rtb_Sum_j4 * rtb_Y_aj; - if (rtb_Divide_kq > PitchNormalLaw_rtP.Saturation_UpperSat_hc) { - rtb_Divide_kq = PitchNormalLaw_rtP.Saturation_UpperSat_hc; - } else if (rtb_Divide_kq < PitchNormalLaw_rtP.Saturation_LowerSat_a) { - rtb_Divide_kq = PitchNormalLaw_rtP.Saturation_LowerSat_a; + rtb_Sum_j4 = (((PitchNormalLaw_rtP.Gain3_Gain_b * rtb_Y_i + rtb_Divide_kq) + rtb_Gain1_e) + rtb_Bias_o) + rtb_Sum_j4 * + rtb_Y_o; + if (rtb_v_target > PitchNormalLaw_rtP.Saturation_UpperSat_hc) { + rtb_v_target = PitchNormalLaw_rtP.Saturation_UpperSat_hc; + } else if (rtb_v_target < PitchNormalLaw_rtP.Saturation_LowerSat_a) { + rtb_v_target = PitchNormalLaw_rtP.Saturation_LowerSat_a; } if (rtb_uDLookupTable > PitchNormalLaw_rtP.Saturation_UpperSat_k) { @@ -1812,40 +1811,39 @@ void PitchNormalLaw::step(const real_T *rtu_In_time_dt, const real_T *rtu_In_nz_ rtb_Sum_j4 = PitchNormalLaw_rtP.Saturation_LowerSat_dw; } - PitchNormalLaw_VoterAttitudeProtection(rtb_Divide_kq, rtb_Product_k + rtb_Sum1_d4 * rtb_uDLookupTable, rtb_Sum_j4, - &rtb_Saturation3); + PitchNormalLaw_VoterAttitudeProtection(rtb_v_target, rtb_Product_k + rtb_Sum1_d4 * rtb_uDLookupTable, rtb_Sum_j4, + &rtb_Y_i); rtb_Divide_cq = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs1_Gain_k * *rtu_In_qk_deg_s; - rtb_Product1_ck = (rtb_Divide_cq - PitchNormalLaw_DWork.Delay_DSTATE_ho) / *rtu_In_time_dt; + rtb_Product1_ck = (rtb_Divide_cq - PitchNormalLaw_DWork.Delay_DSTATE_h) / *rtu_In_time_dt; rtb_Divide_l = PitchNormalLaw_rtP.Gain1_Gain_lk * *rtu_In_qk_deg_s; rtb_Sum_j4 = look1_binlxpw(*rtu_In_V_tas_kn, PitchNormalLaw_rtP.uDLookupTable_bp01Data_m, PitchNormalLaw_rtP.uDLookupTable_tableData_a, 6U); rtb_uDLookupTable = *rtu_In_V_tas_kn; - rtb_Divide_kq = PitchNormalLaw_rtP.Gain3_Gain_g * PitchNormalLaw_rtP.Theta_max3_Value - rtb_Loaddemand2; + rtb_v_target = PitchNormalLaw_rtP.Gain3_Gain_g * PitchNormalLaw_rtP.Theta_max3_Value - rtb_Saturation3; if (rtb_uDLookupTable > PitchNormalLaw_rtP.Saturation3_UpperSat_e) { rtb_uDLookupTable = PitchNormalLaw_rtP.Saturation3_UpperSat_e; } else if (rtb_uDLookupTable < PitchNormalLaw_rtP.Saturation3_LowerSat_k) { rtb_uDLookupTable = PitchNormalLaw_rtP.Saturation3_LowerSat_k; } - if (rtb_Divide_kq > PitchNormalLaw_rtP.Saturation2_UpperSat) { - rtb_Divide_kq = PitchNormalLaw_rtP.Saturation2_UpperSat; - } else if (rtb_Divide_kq < PitchNormalLaw_rtP.Saturation2_LowerSat) { - rtb_Divide_kq = PitchNormalLaw_rtP.Saturation2_LowerSat; + if (rtb_v_target > PitchNormalLaw_rtP.Saturation2_UpperSat) { + rtb_v_target = PitchNormalLaw_rtP.Saturation2_UpperSat; + } else if (rtb_v_target < PitchNormalLaw_rtP.Saturation2_LowerSat) { + rtb_v_target = PitchNormalLaw_rtP.Saturation2_LowerSat; } - rtb_Sum_j4 = (PitchNormalLaw_rtP.Gain_Gain_jq * PitchNormalLaw_rtP.Vm_currentms_Value_b * rtb_Divide_l + rtb_Gain_g) - - (rtb_Sum_j4 / (PitchNormalLaw_rtP.Gain5_Gain_m * rtb_uDLookupTable) + PitchNormalLaw_rtP.Bias_Bias_m) * ((rtb_Y_cm + - look1_binlxpw(rtb_Divide_kq, PitchNormalLaw_rtP.Loaddemand2_bp01Data, PitchNormalLaw_rtP.Loaddemand2_tableData, 2U)) - - rtb_Y_cm); - rtb_Y_aj = look1_binlxpw(*rtu_In_V_tas_kn, PitchNormalLaw_rtP.PLUT_bp01Data_e, PitchNormalLaw_rtP.PLUT_tableData_g, 1U); - rtb_Y_cm = rtb_Sum_j4 * rtb_Y_aj; - rtb_Y_aj = look1_binlxpw(*rtu_In_V_tas_kn, PitchNormalLaw_rtP.DLUT_bp01Data_hw, PitchNormalLaw_rtP.DLUT_tableData_l, - 1U); - rtb_Loaddemand2 = rtb_Sum_j4 * rtb_Y_aj * PitchNormalLaw_rtP.DiscreteDerivativeVariableTs_Gain_c; - rtb_Divide_l = (rtb_Loaddemand2 - PitchNormalLaw_DWork.Delay_DSTATE_ds) / *rtu_In_time_dt; - rtb_Gain_g = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs2_Gain_p * *rtu_In_V_tas_kn; + rtb_Sum_j4 = (PitchNormalLaw_rtP.Gain_Gain_jq * PitchNormalLaw_rtP.Vm_currentms_Value_b * rtb_Divide_l + rtb_Gain_mi) + - (rtb_Sum_j4 / (PitchNormalLaw_rtP.Gain5_Gain_m * rtb_uDLookupTable) + PitchNormalLaw_rtP.Bias_Bias_m) * + ((rtb_Divide1_e + look1_binlxpw(rtb_v_target, PitchNormalLaw_rtP.Loaddemand2_bp01Data, + PitchNormalLaw_rtP.Loaddemand2_tableData, 2U)) - rtb_Divide1_e); + rtb_Y_o = look1_binlxpw(*rtu_In_V_tas_kn, PitchNormalLaw_rtP.PLUT_bp01Data_e, PitchNormalLaw_rtP.PLUT_tableData_g, 1U); + rtb_Saturation3 = rtb_Sum_j4 * rtb_Y_o; + rtb_Y_o = look1_binlxpw(*rtu_In_V_tas_kn, PitchNormalLaw_rtP.DLUT_bp01Data_hw, PitchNormalLaw_rtP.DLUT_tableData_l, 1U); + rtb_Divide1_e = rtb_Sum_j4 * rtb_Y_o * PitchNormalLaw_rtP.DiscreteDerivativeVariableTs_Gain_c; + rtb_Divide_l = (rtb_Divide1_e - PitchNormalLaw_DWork.Delay_DSTATE_ds) / *rtu_In_time_dt; + rtb_Gain_mi = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs2_Gain_p * *rtu_In_V_tas_kn; rtb_Sum_j4 = PitchNormalLaw_DWork.Delay_DSTATE_jt; - rtb_uDLookupTable = (rtb_Gain_g - PitchNormalLaw_DWork.Delay_DSTATE_jt) / *rtu_In_time_dt; + rtb_uDLookupTable = (rtb_Gain_mi - PitchNormalLaw_DWork.Delay_DSTATE_jt) / *rtu_In_time_dt; PitchNormalLaw_LagFilter_n(rtb_uDLookupTable, PitchNormalLaw_rtP.LagFilter_C1_f, rtu_In_time_dt, &rtb_Sum_j4, &PitchNormalLaw_DWork.sf_LagFilter_n); if (rtb_Sum_j4 > PitchNormalLaw_rtP.SaturationV_dot_UpperSat_j2) { @@ -1854,10 +1852,10 @@ void PitchNormalLaw::step(const real_T *rtu_In_time_dt, const real_T *rtu_In_nz_ rtb_Sum_j4 = PitchNormalLaw_rtP.SaturationV_dot_LowerSat_n; } - rtb_Gain_f = PitchNormalLaw_rtP.Gain_Gain_l0 * rtb_Sum_j4; + rtb_Gain_ma = PitchNormalLaw_rtP.Gain_Gain_l0 * rtb_Sum_j4; PitchNormalLaw_WashoutFilter(rtb_Gain5_gq, PitchNormalLaw_rtP.WashoutFilter_C1_j, rtu_In_time_dt, &rtb_Sum_j4, &PitchNormalLaw_DWork.sf_WashoutFilter); - rtb_Y_aj = look1_binlxpw(*rtu_In_H_radio_ft, PitchNormalLaw_rtP.ScheduledGain_BreakpointsForDimension1_b, + rtb_Y_o = look1_binlxpw(*rtu_In_H_radio_ft, PitchNormalLaw_rtP.ScheduledGain_BreakpointsForDimension1_b, PitchNormalLaw_rtP.ScheduledGain_Table_e, 3U); rtb_uDLookupTable = (((PitchNormalLaw_rtP.Gain3_Gain * rtb_Divide + rtb_Product1_dm) + rtb_Divide_o) + PitchNormalLaw_rtP.Gain_Gain_l * rtb_Divide_a0) + rtb_Product_kz; @@ -1867,37 +1865,37 @@ void PitchNormalLaw::step(const real_T *rtu_In_time_dt, const real_T *rtu_In_nz_ rtb_Sum_j4 = PitchNormalLaw_rtP.SaturationSpoilers_LowerSat_d; } - rtb_Divide_kq = (((PitchNormalLaw_rtP.Gain3_Gain_n * rtb_Product1_ck + rtb_Y_cm) + rtb_Divide_l) + rtb_Gain_f) + - rtb_Sum_j4 * rtb_Y_aj; + rtb_v_target = (((PitchNormalLaw_rtP.Gain3_Gain_n * rtb_Product1_ck + rtb_Saturation3) + rtb_Divide_l) + rtb_Gain_ma) + + rtb_Sum_j4 * rtb_Y_o; if (rtb_uDLookupTable > PitchNormalLaw_rtP.Saturation_UpperSat_h) { rtb_uDLookupTable = PitchNormalLaw_rtP.Saturation_UpperSat_h; } else if (rtb_uDLookupTable < PitchNormalLaw_rtP.Saturation_LowerSat_o) { rtb_uDLookupTable = PitchNormalLaw_rtP.Saturation_LowerSat_o; } - if (rtb_Divide_kq > PitchNormalLaw_rtP.Saturation_UpperSat_a) { - rtb_Divide_kq = PitchNormalLaw_rtP.Saturation_UpperSat_a; - } else if (rtb_Divide_kq < PitchNormalLaw_rtP.Saturation_LowerSat_k) { - rtb_Divide_kq = PitchNormalLaw_rtP.Saturation_LowerSat_k; + if (rtb_v_target > PitchNormalLaw_rtP.Saturation_UpperSat_a) { + rtb_v_target = PitchNormalLaw_rtP.Saturation_UpperSat_a; + } else if (rtb_v_target < PitchNormalLaw_rtP.Saturation_LowerSat_k) { + rtb_v_target = PitchNormalLaw_rtP.Saturation_LowerSat_k; } - PitchNormalLaw_VoterAttitudeProtection(rtb_uDLookupTable, rtb_Saturation3, rtb_Divide_kq, &rtb_Y_aj); + PitchNormalLaw_VoterAttitudeProtection(rtb_uDLookupTable, rtb_Y_i, rtb_v_target, &rtb_Y_o); rtb_Sum_j4 = look1_binlxpw(*rtu_In_V_ias_kn, PitchNormalLaw_rtP.ScheduledGain1_BreakpointsForDimension1, PitchNormalLaw_rtP.ScheduledGain1_Table, 4U); - rtb_Y_cm = rtb_Y_aj * rtb_Sum_j4; + rtb_Divide = rtb_Y_o * rtb_Sum_j4; rtb_Sum_j4 = look1_binlxpw(*rtu_In_time_dt, PitchNormalLaw_rtP.ScheduledGain_BreakpointsForDimension1_d, PitchNormalLaw_rtP.ScheduledGain_Table_hh, 4U); - rtb_Sum_j4 = rtb_Y_cm * rtb_Sum_j4 * PitchNormalLaw_rtP.DiscreteTimeIntegratorVariableTs_Gain * *rtu_In_time_dt; + rtb_Sum_j4 = rtb_Divide * rtb_Sum_j4 * PitchNormalLaw_rtP.DiscreteTimeIntegratorVariableTs_Gain * *rtu_In_time_dt; + rtb_OR = ((rtb_Y_o2 == 0.0) || (*rtu_In_tracking_mode_on)); if (*rtu_In_in_flight > PitchNormalLaw_rtP.Switch_Threshold) { - rtb_Y_aj = *rtu_In_eta_deg; + rtb_Y_o = *rtu_In_eta_deg; } else { - rtb_Y_aj = rtb_Gain; + rtb_Y_o = rtb_Gain1; } - rtb_OR = ((rtb_Y_b == 0.0) || (*rtu_In_tracking_mode_on)); PitchNormalLaw_DWork.icLoad = (rtb_OR || PitchNormalLaw_DWork.icLoad); if (PitchNormalLaw_DWork.icLoad) { - PitchNormalLaw_DWork.Delay_DSTATE_o = rtb_Y_aj - rtb_Sum_j4; + PitchNormalLaw_DWork.Delay_DSTATE_o = rtb_Y_o - rtb_Sum_j4; } PitchNormalLaw_DWork.Delay_DSTATE_o += rtb_Sum_j4; @@ -1908,70 +1906,69 @@ void PitchNormalLaw::step(const real_T *rtu_In_time_dt, const real_T *rtu_In_nz_ } if (rtb_eta_trim_deg_should_freeze == PitchNormalLaw_rtP.CompareToConstant_const) { - rtb_Sum_j4 = PitchNormalLaw_rtP.Constant_Value; + rtb_v_target = PitchNormalLaw_rtP.Constant_Value; } else { - rtb_Sum_j4 = PitchNormalLaw_DWork.Delay_DSTATE_o; + rtb_v_target = PitchNormalLaw_DWork.Delay_DSTATE_o; } - rtb_Sum_j4 *= PitchNormalLaw_rtP.Gain_Gain_cy; - if (rtb_Sum_j4 > rtb_eta_trim_deg_rate_limit_up_deg_s) { + rtb_Divide = PitchNormalLaw_rtP.Gain_Gain_cy * rtb_v_target; + if (rtb_Divide > rtb_eta_trim_deg_rate_limit_up_deg_s) { *rty_Out_eta_trim_dot_deg_s = rtb_eta_trim_deg_rate_limit_up_deg_s; - } else if (rtb_Sum_j4 < rtb_eta_trim_deg_rate_limit_lo_deg_s) { + } else if (rtb_Divide < rtb_eta_trim_deg_rate_limit_lo_deg_s) { *rty_Out_eta_trim_dot_deg_s = rtb_eta_trim_deg_rate_limit_lo_deg_s; } else { - *rty_Out_eta_trim_dot_deg_s = rtb_Sum_j4; + *rty_Out_eta_trim_dot_deg_s = rtb_Divide; } - if (rtb_Y_b > PitchNormalLaw_rtP.Saturation_UpperSat_l) { + if (rtb_Y_o2 > PitchNormalLaw_rtP.Saturation_UpperSat_l) { rtb_Sum_j4 = PitchNormalLaw_rtP.Saturation_UpperSat_l; - } else if (rtb_Y_b < PitchNormalLaw_rtP.Saturation_LowerSat_kp) { + } else if (rtb_Y_o2 < PitchNormalLaw_rtP.Saturation_LowerSat_kp) { rtb_Sum_j4 = PitchNormalLaw_rtP.Saturation_LowerSat_kp; } else { - rtb_Sum_j4 = rtb_Y_b; + rtb_Sum_j4 = rtb_Y_o2; } - rtb_Product1_dm = PitchNormalLaw_DWork.Delay_DSTATE_o * rtb_Sum_j4; - rtb_Divide = PitchNormalLaw_rtP.Constant_Value_o1 - rtb_Sum_j4; + rtb_eta_trim_deg_rate_limit_lo_deg_s = PitchNormalLaw_DWork.Delay_DSTATE_o * rtb_Sum_j4; + rtb_Product1_dm = PitchNormalLaw_rtP.Constant_Value_o1 - rtb_Sum_j4; PitchNormalLaw_RateLimiter_c(rtu_In_delta_eta_pos, PitchNormalLaw_rtP.RateLimiterVariableTs_up_na, PitchNormalLaw_rtP.RateLimiterVariableTs_lo_i, rtu_In_time_dt, - PitchNormalLaw_rtP.RateLimiterVariableTs_InitialCondition_m, &rtb_Y_cm, &PitchNormalLaw_DWork.sf_RateLimiter_ct); + PitchNormalLaw_rtP.RateLimiterVariableTs_InitialCondition_m, &rtb_Y_i, &PitchNormalLaw_DWork.sf_RateLimiter_ct); rtb_Sum_j4 = look2_binlxpw(*rtu_In_Theta_deg, *rtu_In_H_radio_ft, PitchNormalLaw_rtP.uDLookupTable_bp01Data_l, PitchNormalLaw_rtP.uDLookupTable_bp02Data, PitchNormalLaw_rtP.uDLookupTable_tableData_e5, PitchNormalLaw_rtP.uDLookupTable_maxIndex, 5U); - rtb_Y_aj = *rtu_In_tailstrike_protection_on; - if (rtb_Y_cm > PitchNormalLaw_rtP.Saturation3_UpperSat_l) { - rtb_eta_trim_deg_rate_limit_up_deg_s = PitchNormalLaw_rtP.Saturation3_UpperSat_l; - } else if (rtb_Y_cm < PitchNormalLaw_rtP.Saturation3_LowerSat_h) { - rtb_eta_trim_deg_rate_limit_up_deg_s = PitchNormalLaw_rtP.Saturation3_LowerSat_h; + rtb_Y_o = *rtu_In_tailstrike_protection_on; + if (rtb_Y_i > PitchNormalLaw_rtP.Saturation3_UpperSat_l) { + rtb_v_target = PitchNormalLaw_rtP.Saturation3_UpperSat_l; + } else if (rtb_Y_i < PitchNormalLaw_rtP.Saturation3_LowerSat_h) { + rtb_v_target = PitchNormalLaw_rtP.Saturation3_LowerSat_h; } else { - rtb_eta_trim_deg_rate_limit_up_deg_s = rtb_Y_cm; + rtb_v_target = rtb_Y_i; } - rtb_Sum_j4 = look1_binlxpw(rtb_Y_aj * rtb_Sum_j4 * rtb_eta_trim_deg_rate_limit_up_deg_s + rtb_Y_cm, - PitchNormalLaw_rtP.PitchRateDemand_bp01Data, PitchNormalLaw_rtP.PitchRateDemand_tableData, 2U); - rtb_eta_trim_deg_rate_limit_up_deg_s = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs_Gain_j3 * rtb_Sum_j4; - rtb_Y_cm = (rtb_eta_trim_deg_rate_limit_up_deg_s - PitchNormalLaw_DWork.Delay_DSTATE_ej) / *rtu_In_time_dt; - rtb_Y_aj = *rtu_In_qk_deg_s - rtb_Sum_j4; - rtb_Divide_o = PitchNormalLaw_rtP.Gain_Gain_pt * rtb_Y_aj; - rtb_eta_trim_deg_rate_limit_lo_deg_s = PitchNormalLaw_rtP.Gain1_Gain_d * rtb_Y_aj * + rtb_Sum_j4 = look1_binlxpw(rtb_Y_o * rtb_Sum_j4 * rtb_v_target + rtb_Y_i, PitchNormalLaw_rtP.PitchRateDemand_bp01Data, + PitchNormalLaw_rtP.PitchRateDemand_tableData, 2U); + rtb_Divide = PitchNormalLaw_rtP.DiscreteDerivativeVariableTs_Gain_j3 * rtb_Sum_j4; + rtb_Saturation3 = (rtb_Divide - PitchNormalLaw_DWork.Delay_DSTATE_ej) / *rtu_In_time_dt; + rtb_Y_o = *rtu_In_qk_deg_s - rtb_Sum_j4; + rtb_Divide_o = PitchNormalLaw_rtP.Gain_Gain_pt * rtb_Y_o; + rtb_eta_trim_deg_rate_limit_up_deg_s = PitchNormalLaw_rtP.Gain1_Gain_d * rtb_Y_o * PitchNormalLaw_rtP.DiscreteDerivativeVariableTs_Gain_g; - rtb_Y_aj = PitchNormalLaw_DWork.Delay_DSTATE_e4; - rtb_Divide_a0 = (rtb_eta_trim_deg_rate_limit_lo_deg_s - PitchNormalLaw_DWork.Delay_DSTATE_e4) / *rtu_In_time_dt; + rtb_Y_o = PitchNormalLaw_DWork.Delay_DSTATE_e4; + rtb_Divide_a0 = (rtb_eta_trim_deg_rate_limit_up_deg_s - PitchNormalLaw_DWork.Delay_DSTATE_e4) / *rtu_In_time_dt; rtb_Gain5_gq = PitchNormalLaw_rtP.Gain5_Gain_h * *rtu_In_qk_dot_deg_s2; rtb_Gain5_gq += *rtu_In_qk_deg_s; - PitchNormalLaw_LagFilter_n(rtb_Gain5_gq, PitchNormalLaw_rtP.LagFilter_C1_k, rtu_In_time_dt, &rtb_Y_aj, + PitchNormalLaw_LagFilter_n(rtb_Gain5_gq, PitchNormalLaw_rtP.LagFilter_C1_k, rtu_In_time_dt, &rtb_Y_o, &PitchNormalLaw_DWork.sf_LagFilter_f); rtb_Gain5_gq = PitchNormalLaw_rtP.Gain6_Gain_g * *rtu_In_qk_dot_deg_s2; - rtb_Y_cm = (((rtb_Divide_o + rtb_Divide_a0) * PitchNormalLaw_rtP.Gain1_Gain_a + PitchNormalLaw_rtP.Gain3_Gain_e * - rtb_Y_cm) + (rtb_Y_aj - rtb_Sum_j4) * PitchNormalLaw_rtP.Gain4_Gain) + rtb_Gain5_gq; + rtb_Saturation3 = (((rtb_Divide_o + rtb_Divide_a0) * PitchNormalLaw_rtP.Gain1_Gain_a + PitchNormalLaw_rtP.Gain3_Gain_e + * rtb_Saturation3) + (rtb_Y_o - rtb_Sum_j4) * PitchNormalLaw_rtP.Gain4_Gain) + rtb_Gain5_gq; rtb_Sum_j4 = look1_binlxpw(*rtu_In_V_ias_kn, PitchNormalLaw_rtP.ScheduledGain1_BreakpointsForDimension1_h, PitchNormalLaw_rtP.ScheduledGain1_Table_c, 4U); - rtb_Sum_j4 = (PitchNormalLaw_rtP.Constant2_Value_k - rtb_Y_b) * (rtb_Y_cm * rtb_Sum_j4) * + rtb_Sum_j4 = (PitchNormalLaw_rtP.Constant2_Value_k - rtb_Y_o2) * (rtb_Saturation3 * rtb_Sum_j4) * PitchNormalLaw_rtP.DiscreteTimeIntegratorVariableTs_Gain_j * *rtu_In_time_dt; rtb_eta_trim_deg_should_freeze = (*rtu_In_delta_eta_pos <= PitchNormalLaw_rtP.Constant_Value_o); rtb_eta_trim_deg_should_freeze = (rtb_eta_trim_deg_should_freeze && (*rtu_In_on_ground)); - rtb_eta_trim_deg_should_freeze = (rtb_eta_trim_deg_should_freeze || (rtb_ManualSwitch == 0.0) || - (*rtu_In_tracking_mode_on)); + rtb_eta_trim_deg_should_freeze = (rtb_eta_trim_deg_should_freeze || (rtb_Y_bc == 0.0) || (*rtu_In_tracking_mode_on)); PitchNormalLaw_DWork.icLoad_p = (rtb_eta_trim_deg_should_freeze || PitchNormalLaw_DWork.icLoad_p); if (PitchNormalLaw_DWork.icLoad_p) { PitchNormalLaw_DWork.Delay_DSTATE_cl = PitchNormalLaw_rtP.Constant_Value_jk - rtb_Sum_j4; @@ -1985,28 +1982,28 @@ void PitchNormalLaw::step(const real_T *rtu_In_time_dt, const real_T *rtu_In_nz_ } if (*rtu_In_on_ground) { - if (rtb_Gain > PitchNormalLaw_rtP.Saturation_UpperSat_f) { + if (rtb_Gain1 > PitchNormalLaw_rtP.Saturation_UpperSat_f) { rtb_Sum_j4 = PitchNormalLaw_rtP.Saturation_UpperSat_f; - } else if (rtb_Gain < PitchNormalLaw_rtP.Saturation_LowerSat_p) { + } else if (rtb_Gain1 < PitchNormalLaw_rtP.Saturation_LowerSat_p) { rtb_Sum_j4 = PitchNormalLaw_rtP.Saturation_LowerSat_p; } else { - rtb_Sum_j4 = rtb_Gain; + rtb_Sum_j4 = rtb_Gain1; } } else { rtb_Sum_j4 = PitchNormalLaw_rtP.Constant1_Value_h; } - rtb_Y_b = PitchNormalLaw_DWork.Delay_DSTATE_cl + rtb_Sum_j4; - if (rtb_ManualSwitch > PitchNormalLaw_rtP.Saturation_UpperSat_m) { + rtb_Y_o2 = PitchNormalLaw_DWork.Delay_DSTATE_cl + rtb_Sum_j4; + if (rtb_Y_bc > PitchNormalLaw_rtP.Saturation_UpperSat_m) { rtb_Sum_j4 = PitchNormalLaw_rtP.Saturation_UpperSat_m; - } else if (rtb_ManualSwitch < PitchNormalLaw_rtP.Saturation_LowerSat_b) { + } else if (rtb_Y_bc < PitchNormalLaw_rtP.Saturation_LowerSat_b) { rtb_Sum_j4 = PitchNormalLaw_rtP.Saturation_LowerSat_b; } else { - rtb_Sum_j4 = rtb_ManualSwitch; + rtb_Sum_j4 = rtb_Y_bc; } - rtb_uDLookupTable = ((PitchNormalLaw_rtP.Constant_Value_h - rtb_Sum_j4) * rtb_Gain + rtb_Y_b * rtb_Sum_j4) * - rtb_Divide + rtb_Product1_dm; + rtb_uDLookupTable = ((PitchNormalLaw_rtP.Constant_Value_h - rtb_Sum_j4) * rtb_Gain1 + rtb_Y_o2 * rtb_Sum_j4) * + rtb_Product1_dm + rtb_eta_trim_deg_rate_limit_lo_deg_s; if (rtb_uDLookupTable > PitchNormalLaw_rtP.Saturation_UpperSat_kp) { rtb_uDLookupTable = PitchNormalLaw_rtP.Saturation_UpperSat_kp; } else if (rtb_uDLookupTable < PitchNormalLaw_rtP.Saturation_LowerSat_a4) { @@ -2016,12 +2013,12 @@ void PitchNormalLaw::step(const real_T *rtu_In_time_dt, const real_T *rtu_In_nz_ PitchNormalLaw_RateLimiter(rtb_uDLookupTable, PitchNormalLaw_rtP.RateLimitereta_up, PitchNormalLaw_rtP.RateLimitereta_lo, rtu_In_time_dt, PitchNormalLaw_rtP.RateLimitereta_InitialCondition, rty_Out_eta_deg, &PitchNormalLaw_DWork.sf_RateLimiter_b); - PitchNormalLaw_DWork.Delay_DSTATE_h = rtb_Loaddemand2_l; - PitchNormalLaw_DWork.Delay_DSTATE_n = rtb_Gain_px; - PitchNormalLaw_DWork.Delay_DSTATE_c = rtb_Gain_ot; + PitchNormalLaw_DWork.Delay_DSTATE = rtb_ManualSwitch; + PitchNormalLaw_DWork.Delay_DSTATE_n = rtb_Gain1_h; + PitchNormalLaw_DWork.Delay_DSTATE_c = rtb_Gain_ci; PitchNormalLaw_DWork.Delay_DSTATE_l = rtb_Divide_an; - PitchNormalLaw_DWork.Delay_DSTATE_k = rtb_Gain1_ft; - PitchNormalLaw_DWork.Delay_DSTATE_d = rtb_Gain_bs; + PitchNormalLaw_DWork.Delay_DSTATE_k = rtb_Gain1_g; + PitchNormalLaw_DWork.Delay_DSTATE_d = rtb_Saturation3_i; PitchNormalLaw_DWork.Delay_DSTATE_f = rtb_Divide_m; PitchNormalLaw_DWork.Delay_DSTATE_g = rtb_Sum10; PitchNormalLaw_DWork.Delay_DSTATE_j = rtb_y; @@ -2029,19 +2026,19 @@ void PitchNormalLaw::step(const real_T *rtu_In_time_dt, const real_T *rtu_In_nz_ PitchNormalLaw_DWork.Delay_DSTATE_kd = rtb_alpha_err_gain; PitchNormalLaw_DWork.Delay_DSTATE_ku = rtb_qk_gain; PitchNormalLaw_DWork.Delay_DSTATE_gl = rtb_qk_dot_gain; - PitchNormalLaw_DWork.Delay_DSTATE_m = rtb_Gain_ll; + PitchNormalLaw_DWork.Delay_DSTATE_m = rtb_Gain_ka; PitchNormalLaw_DWork.Delay_DSTATE_k2 = rtb_Divide_b4; PitchNormalLaw_DWork.Delay_DSTATE_jh = rtb_Cos; - PitchNormalLaw_DWork.Delay_DSTATE_dy = rtb_Y_p; - PitchNormalLaw_DWork.Delay_DSTATE_e5 = rtb_Y_n; - PitchNormalLaw_DWork.Delay_DSTATE_gz = rtb_Y_ll; - PitchNormalLaw_DWork.Delay_DSTATE_lf = rtb_Y_am; - PitchNormalLaw_DWork.Delay_DSTATE_ho = rtb_Divide_cq; - PitchNormalLaw_DWork.Delay_DSTATE_ds = rtb_Loaddemand2; - PitchNormalLaw_DWork.Delay_DSTATE_jt = rtb_Gain_g; + PitchNormalLaw_DWork.Delay_DSTATE_dy = rtb_Y_o4; + PitchNormalLaw_DWork.Delay_DSTATE_e5 = rtb_Y_p; + PitchNormalLaw_DWork.Delay_DSTATE_gz = rtb_Y_b; + PitchNormalLaw_DWork.Delay_DSTATE_lf = rtb_Y_l; + PitchNormalLaw_DWork.Delay_DSTATE_h = rtb_Divide_cq; + PitchNormalLaw_DWork.Delay_DSTATE_ds = rtb_Divide1_e; + PitchNormalLaw_DWork.Delay_DSTATE_jt = rtb_Gain_mi; PitchNormalLaw_DWork.icLoad = false; - PitchNormalLaw_DWork.Delay_DSTATE_ej = rtb_eta_trim_deg_rate_limit_up_deg_s; - PitchNormalLaw_DWork.Delay_DSTATE_e4 = rtb_eta_trim_deg_rate_limit_lo_deg_s; + PitchNormalLaw_DWork.Delay_DSTATE_ej = rtb_Divide; + PitchNormalLaw_DWork.Delay_DSTATE_e4 = rtb_eta_trim_deg_rate_limit_up_deg_s; PitchNormalLaw_DWork.icLoad_p = false; } diff --git a/hdw-a339x/src/wasm/fbw_a320/src/model/PitchNormalLaw.h b/hdw-a339x/src/wasm/fbw_a320/src/model/PitchNormalLaw.h new file mode 100644 index 000000000..66e8b9833 --- /dev/null +++ b/hdw-a339x/src/wasm/fbw_a320/src/model/PitchNormalLaw.h @@ -0,0 +1,525 @@ +#ifndef RTW_HEADER_PitchNormalLaw_h_ +#define RTW_HEADER_PitchNormalLaw_h_ +#include "rtwtypes.h" +#include "PitchNormalLaw_types.h" +#include + +class PitchNormalLaw final +{ + public: + struct rtDW_LagFilter_PitchNormalLaw_T { + real_T pY; + real_T pU; + boolean_T pY_not_empty; + boolean_T pU_not_empty; + }; + + struct rtDW_RateLimiter_PitchNormalLaw_T { + real_T pY; + boolean_T pY_not_empty; + }; + + struct rtDW_eta_trim_limit_lofreeze_PitchNormalLaw_T { + real_T frozen_eta_trim; + boolean_T frozen_eta_trim_not_empty; + }; + + struct rtDW_LagFilter_PitchNormalLaw_d_T { + real_T pY; + real_T pU; + boolean_T pY_not_empty; + boolean_T pU_not_empty; + }; + + struct rtDW_WashoutFilter_PitchNormalLaw_T { + real_T pY; + real_T pU; + boolean_T pY_not_empty; + boolean_T pU_not_empty; + }; + + struct rtDW_RateLimiter_PitchNormalLaw_o_T { + real_T pY; + boolean_T pY_not_empty; + }; + + struct BlockIO_PitchNormalLaw_T { + real_T flare_Theta_c_deg; + real_T flare_Theta_c_rate_deg_s; + }; + + struct D_Work_PitchNormalLaw_T { + real_T Delay_DSTATE; + real_T Delay_DSTATE_e; + real_T Delay_DSTATE_n; + real_T Delay_DSTATE_c; + real_T Delay_DSTATE_l; + real_T Delay_DSTATE_k; + real_T Delay_DSTATE_d; + real_T Delay_DSTATE_f; + real_T Delay_DSTATE_g; + real_T Delay1_DSTATE; + real_T Delay_DSTATE_j; + real_T Delay_DSTATE_ca; + real_T Delay1_DSTATE_i; + real_T Delay_DSTATE_e1; + real_T Delay_DSTATE_kd; + real_T Delay_DSTATE_b; + real_T Delay_DSTATE_ku; + real_T Delay_DSTATE_gl; + real_T Delay1_DSTATE_l; + real_T Delay_DSTATE_m; + real_T Delay_DSTATE_k2; + real_T Delay1_DSTATE_n; + real_T Delay_DSTATE_mz; + real_T Delay_DSTATE_jh; + real_T Delay_DSTATE_dy; + real_T Delay_DSTATE_e5; + real_T Delay_DSTATE_gz; + real_T Delay_DSTATE_lf; + real_T Delay_DSTATE_h; + real_T Delay_DSTATE_ds; + real_T Delay_DSTATE_jt; + real_T Delay_DSTATE_o; + real_T Delay_DSTATE_ej; + real_T Delay_DSTATE_e4; + real_T Delay_DSTATE_cl; + uint8_T is_active_c6_PitchNormalLaw; + uint8_T is_c6_PitchNormalLaw; + uint8_T is_active_c7_PitchNormalLaw; + uint8_T is_c7_PitchNormalLaw; + uint8_T is_active_c8_PitchNormalLaw; + uint8_T is_c8_PitchNormalLaw; + uint8_T is_active_c9_PitchNormalLaw; + uint8_T is_c9_PitchNormalLaw; + uint8_T is_active_c2_PitchNormalLaw; + uint8_T is_c2_PitchNormalLaw; + boolean_T icLoad; + boolean_T icLoad_p; + rtDW_RateLimiter_PitchNormalLaw_T sf_RateLimiter_b; + rtDW_RateLimiter_PitchNormalLaw_o_T sf_RateLimiter_ct; + rtDW_LagFilter_PitchNormalLaw_d_T sf_LagFilter_f; + rtDW_WashoutFilter_PitchNormalLaw_T sf_WashoutFilter_h; + rtDW_RateLimiter_PitchNormalLaw_T sf_RateLimiter_l; + rtDW_RateLimiter_PitchNormalLaw_o_T sf_RateLimiter_nx; + rtDW_RateLimiter_PitchNormalLaw_o_T sf_RateLimiter_d; + rtDW_RateLimiter_PitchNormalLaw_o_T sf_RateLimiter_c2; + rtDW_RateLimiter_PitchNormalLaw_T sf_RateLimiter_o; + rtDW_LagFilter_PitchNormalLaw_T sf_LagFilter_m; + rtDW_WashoutFilter_PitchNormalLaw_T sf_WashoutFilter_c; + rtDW_LagFilter_PitchNormalLaw_d_T sf_LagFilter_g3; + rtDW_WashoutFilter_PitchNormalLaw_T sf_WashoutFilter_d; + rtDW_LagFilter_PitchNormalLaw_d_T sf_LagFilter_g; + rtDW_WashoutFilter_PitchNormalLaw_T sf_WashoutFilter_l; + rtDW_LagFilter_PitchNormalLaw_d_T sf_LagFilter_i; + rtDW_WashoutFilter_PitchNormalLaw_T sf_WashoutFilter_k; + rtDW_LagFilter_PitchNormalLaw_d_T sf_LagFilter_k; + rtDW_WashoutFilter_PitchNormalLaw_T sf_WashoutFilter; + rtDW_LagFilter_PitchNormalLaw_d_T sf_LagFilter_n; + rtDW_eta_trim_limit_lofreeze_PitchNormalLaw_T sf_eta_trim_limit_upfreeze; + rtDW_eta_trim_limit_lofreeze_PitchNormalLaw_T sf_eta_trim_limit_lofreeze; + rtDW_RateLimiter_PitchNormalLaw_T sf_RateLimiter_n; + rtDW_RateLimiter_PitchNormalLaw_T sf_RateLimiter_c; + rtDW_RateLimiter_PitchNormalLaw_T sf_RateLimiter_p; + rtDW_RateLimiter_PitchNormalLaw_T sf_RateLimiter; + rtDW_LagFilter_PitchNormalLaw_T sf_LagFilter; + }; + + struct Parameters_PitchNormalLaw_T { + real_T ScheduledGain_BreakpointsForDimension1[4]; + real_T ScheduledGain_BreakpointsForDimension1_n[4]; + real_T ScheduledGain_BreakpointsForDimension1_h[7]; + real_T ScheduledGain_BreakpointsForDimension1_c[4]; + real_T ScheduledGain_BreakpointsForDimension1_f[4]; + real_T ScheduledGain_BreakpointsForDimension1_b[4]; + real_T ScheduledGain1_BreakpointsForDimension1[5]; + real_T ScheduledGain_BreakpointsForDimension1_d[5]; + real_T ScheduledGain1_BreakpointsForDimension1_h[5]; + real_T LagFilter_C1; + real_T LagFilter_C1_i; + real_T WashoutFilter_C1; + real_T LagFilter_C1_p; + real_T WashoutFilter_C1_n; + real_T LagFilter1_C1; + real_T WashoutFilter_C1_b; + real_T Subsystem1_C1; + real_T Subsystem3_C1; + real_T Subsystem2_C1; + real_T Subsystem_C1; + real_T LagFilter_C1_pt; + real_T WashoutFilter_C1_l; + real_T LagFilter_C1_l; + real_T WashoutFilter_C1_h; + real_T LagFilter_C1_f; + real_T WashoutFilter_C1_j; + real_T LagFilter_C1_k; + real_T DiscreteDerivativeVariableTs1_Gain; + real_T DiscreteDerivativeVariableTs_Gain; + real_T DiscreteDerivativeVariableTs2_Gain; + real_T DiscreteDerivativeVariableTs1_Gain_i; + real_T DiscreteDerivativeVariableTs_Gain_j; + real_T DiscreteDerivativeVariableTs2_Gain_e; + real_T Subsystem1_Gain; + real_T Subsystem3_Gain; + real_T DiscreteDerivativeVariableTs1_Gain_m; + real_T Subsystem2_Gain; + real_T Subsystem_Gain; + real_T DiscreteDerivativeVariableTs_Gain_b; + real_T DiscreteDerivativeVariableTs2_Gain_c; + real_T DiscreteDerivativeVariableTs1_Gain_c; + real_T DiscreteDerivativeVariableTs_Gain_p; + real_T DiscreteDerivativeVariableTs2_Gain_a; + real_T DiscreteDerivativeVariableTs1_Gain_k; + real_T DiscreteDerivativeVariableTs_Gain_c; + real_T DiscreteDerivativeVariableTs2_Gain_p; + real_T DiscreteTimeIntegratorVariableTs_Gain; + real_T DiscreteDerivativeVariableTs_Gain_j3; + real_T DiscreteDerivativeVariableTs_Gain_g; + real_T DiscreteTimeIntegratorVariableTs_Gain_j; + real_T RateLimiterVariableTs_InitialCondition; + real_T RateLimiterVariableTs1_InitialCondition; + real_T RateLimiterVariableTs2_InitialCondition; + real_T RateLimiterVariableTs3_InitialCondition; + real_T RateLimiterDynamicVariableTs_InitialCondition; + real_T RateLimiterVariableTs6_InitialCondition; + real_T DiscreteDerivativeVariableTs1_InitialCondition; + real_T DiscreteDerivativeVariableTs_InitialCondition; + real_T DiscreteDerivativeVariableTs2_InitialCondition; + real_T DiscreteDerivativeVariableTs1_InitialCondition_l; + real_T DiscreteDerivativeVariableTs_InitialCondition_o; + real_T DiscreteDerivativeVariableTs2_InitialCondition_d; + real_T RateLimiterVariableTs2_InitialCondition_f; + real_T DiscreteDerivativeVariableTs2_InitialCondition_h; + real_T DiscreteDerivativeVariableTs2_InitialCondition_m; + real_T RateLimiterVariableTs5_InitialCondition; + real_T DiscreteDerivativeVariableTs1_InitialCondition_j; + real_T RateLimiterVariableTs1_InitialCondition_l; + real_T RateLimiterVariableTs_InitialCondition_o; + real_T RateLimiterVariableTs3_InitialCondition_e; + real_T DiscreteDerivativeVariableTs2_InitialCondition_f; + real_T DiscreteDerivativeVariableTs2_InitialCondition_e; + real_T RateLimiterVariableTs4_InitialCondition; + real_T RateLimiterVariableTs6_InitialCondition_f; + real_T DiscreteDerivativeVariableTs_InitialCondition_a; + real_T DiscreteDerivativeVariableTs2_InitialCondition_di; + real_T DiscreteDerivativeVariableTs1_InitialCondition_f; + real_T DiscreteDerivativeVariableTs_InitialCondition_g; + real_T DiscreteDerivativeVariableTs2_InitialCondition_c; + real_T DiscreteDerivativeVariableTs1_InitialCondition_g; + real_T DiscreteDerivativeVariableTs_InitialCondition_h; + real_T DiscreteDerivativeVariableTs2_InitialCondition_a; + real_T RateLimiterVariableTs_InitialCondition_m; + real_T DiscreteDerivativeVariableTs_InitialCondition_b; + real_T DiscreteDerivativeVariableTs_InitialCondition_p; + real_T RateLimitereta_InitialCondition; + real_T DiscreteTimeIntegratorVariableTs_LowerLimit; + real_T DiscreteTimeIntegratorVariableTs_LowerLimit_h; + real_T ScheduledGain_Table[4]; + real_T ScheduledGain_Table_b[4]; + real_T ScheduledGain_Table_j[7]; + real_T ScheduledGain_Table_g[4]; + real_T ScheduledGain_Table_h[4]; + real_T ScheduledGain_Table_e[4]; + real_T ScheduledGain1_Table[5]; + real_T ScheduledGain_Table_hh[5]; + real_T ScheduledGain1_Table_c[5]; + real_T DiscreteTimeIntegratorVariableTs_UpperLimit; + real_T DiscreteTimeIntegratorVariableTs_UpperLimit_p; + real_T RateLimiterVariableTs_lo; + real_T RateLimiterVariableTs1_lo; + real_T RateLimiterVariableTs2_lo; + real_T RateLimiterVariableTs3_lo; + real_T RateLimiterVariableTs6_lo; + real_T RateLimiterVariableTs2_lo_k; + real_T RateLimiterVariableTs5_lo; + real_T RateLimiterVariableTs1_lo_g; + real_T RateLimiterVariableTs_lo_c; + real_T RateLimiterVariableTs3_lo_b; + real_T RateLimiterVariableTs4_lo; + real_T RateLimiterVariableTs6_lo_p; + real_T RateLimiterVariableTs_lo_i; + real_T RateLimitereta_lo; + real_T RateLimiterVariableTs_up; + real_T RateLimiterVariableTs1_up; + real_T RateLimiterVariableTs2_up; + real_T RateLimiterVariableTs3_up; + real_T RateLimiterVariableTs6_up; + real_T RateLimiterVariableTs2_up_m; + real_T RateLimiterVariableTs5_up; + real_T RateLimiterVariableTs1_up_d; + real_T RateLimiterVariableTs_up_n; + real_T RateLimiterVariableTs3_up_i; + real_T RateLimiterVariableTs4_up; + real_T RateLimiterVariableTs6_up_n; + real_T RateLimiterVariableTs_up_na; + real_T RateLimitereta_up; + boolean_T CompareToConstant_const; + real_T Constant2_Value; + real_T Constant3_Value; + real_T Constant_Value; + real_T qk_dot_gain1_Gain; + real_T qk_gain_HSP_Gain; + real_T v_dot_gain_HSP_Gain; + real_T Gain6_Gain; + real_T precontrol_gain_HSP_Gain; + real_T HSP_gain_Gain; + real_T Saturation4_UpperSat; + real_T Saturation4_LowerSat; + real_T Saturation8_UpperSat; + real_T Saturation8_LowerSat; + real_T Constant1_Value; + real_T Loaddemand_tableData[3]; + real_T Loaddemand_bp01Data[3]; + real_T Constant_Value_l; + real_T Constant_Value_g; + real_T Saturation_UpperSat; + real_T Saturation_LowerSat; + real_T Gain_Gain; + real_T Saturation_UpperSat_g; + real_T Saturation_LowerSat_d; + real_T Constant_Value_m; + real_T Saturation_UpperSat_e; + real_T Saturation_LowerSat_m; + real_T Switch2_Threshold; + real_T Saturation_UpperSat_f; + real_T Saturation_LowerSat_p; + real_T Constant1_Value_h; + real_T Constant_Value_o; + real_T Constant1_Value_k; + real_T Constant_Value_p; + real_T Saturation_UpperSat_c; + real_T Saturation_LowerSat_n; + real_T Saturation1_UpperSat; + real_T Saturation1_LowerSat; + real_T Gain_Gain_a; + real_T Constant_Value_j; + real_T Constant_Value_c; + real_T Constant_Value_a; + real_T Constant_Value_mx; + real_T Constant_Value_mo; + real_T Gain2_Gain; + real_T Gain1_Gain; + real_T Saturation1_UpperSat_i; + real_T Saturation1_LowerSat_h; + real_T Loaddemand1_tableData[3]; + real_T Loaddemand1_bp01Data[3]; + real_T Gain1_Gain_m; + real_T Gain1_Gain_l; + real_T uDLookupTable_tableData[7]; + real_T uDLookupTable_bp01Data[7]; + real_T Saturation3_UpperSat; + real_T Saturation3_LowerSat; + real_T Gain5_Gain; + real_T Bias_Bias; + real_T Gain1_Gain_e; + real_T Vm_currentms_Value; + real_T Gain_Gain_c; + real_T PLUT_tableData[2]; + real_T PLUT_bp01Data[2]; + real_T Gain3_Gain; + real_T DLUT_tableData[2]; + real_T DLUT_bp01Data[2]; + real_T SaturationV_dot_UpperSat; + real_T SaturationV_dot_LowerSat; + real_T Gain_Gain_l; + real_T SaturationSpoilers_UpperSat; + real_T SaturationSpoilers_LowerSat; + real_T Saturation_UpperSat_h; + real_T Saturation_LowerSat_o; + real_T Gain3_Gain_m; + real_T Gain1_Gain_o; + real_T Vm_currentms_Value_e; + real_T Gain_Gain_al; + real_T uDLookupTable_tableData_e[7]; + real_T uDLookupTable_bp01Data_o[7]; + real_T Saturation3_UpperSat_a; + real_T Saturation3_LowerSat_l; + real_T Gain5_Gain_d; + real_T Bias_Bias_a; + real_T PLUT_tableData_b[2]; + real_T PLUT_bp01Data_b[2]; + real_T DLUT_tableData_p[2]; + real_T DLUT_bp01Data_h[2]; + real_T SaturationV_dot_UpperSat_j; + real_T SaturationV_dot_LowerSat_e; + real_T Gain_Gain_j; + real_T SaturationSpoilers_UpperSat_g; + real_T SaturationSpoilers_LowerSat_j; + real_T Saturation_UpperSat_hc; + real_T Saturation_LowerSat_a; + real_T Delay_InitialCondition; + real_T Constant_Value_f; + real_T Delay1_InitialCondition; + real_T precontrol_gain_Gain; + real_T alpha_err_gain_Gain; + real_T Delay_InitialCondition_e; + real_T Constant_Value_b; + real_T Delay1_InitialCondition_g; + real_T v_dot_gain_Gain; + real_T qk_gain_Gain; + real_T qk_dot_gain_Gain; + real_T Saturation3_UpperSat_f; + real_T Saturation3_LowerSat_c; + real_T Saturation_UpperSat_eo; + real_T Saturation_LowerSat_h; + real_T Constant_Value_fe; + real_T Gain3_Gain_c; + real_T Gain1_Gain_en; + real_T Vm_currentms_Value_h; + real_T Gain_Gain_b; + real_T uDLookupTable_tableData_h[7]; + real_T uDLookupTable_bp01Data_b[7]; + real_T Saturation3_UpperSat_b; + real_T Saturation3_LowerSat_e; + real_T Gain5_Gain_e; + real_T Bias_Bias_f; + real_T Delay_InitialCondition_c; + real_T Constant_Value_ja; + real_T Delay1_InitialCondition_gf; + real_T Delay_InitialCondition_h; + real_T Constant_Value_jj; + real_T Delay1_InitialCondition_e; + real_T Saturation_UpperSat_f1; + real_T Saturation_LowerSat_o1; + real_T Gain1_Gain_lm; + real_T PLUT_tableData_k[2]; + real_T PLUT_bp01Data_f[2]; + real_T DLUT_tableData_a[2]; + real_T DLUT_bp01Data_m[2]; + real_T SaturationV_dot_UpperSat_b; + real_T SaturationV_dot_LowerSat_m; + real_T Gain_Gain_f; + real_T SaturationSpoilers_UpperSat_o; + real_T SaturationSpoilers_LowerSat_jl; + real_T Saturation_UpperSat_k; + real_T Saturation_LowerSat_p1; + real_T Gain3_Gain_b; + real_T Gain1_Gain_b; + real_T Vm_currentms_Value_p; + real_T Gain_Gain_p; + real_T uDLookupTable_tableData_p[7]; + real_T uDLookupTable_bp01Data_a[7]; + real_T Saturation3_UpperSat_n; + real_T Saturation3_LowerSat_a; + real_T Gain5_Gain_n; + real_T Bias_Bias_ai; + real_T PLUT_tableData_o[2]; + real_T PLUT_bp01Data_a[2]; + real_T DLUT_tableData_e[2]; + real_T DLUT_bp01Data_k[2]; + real_T SaturationV_dot_UpperSat_m; + real_T SaturationV_dot_LowerSat_ek; + real_T Gain_Gain_k; + real_T SaturationSpoilers_UpperSat_h; + real_T SaturationSpoilers_LowerSat_l; + real_T Saturation_UpperSat_j; + real_T Saturation_LowerSat_dw; + real_T Gain3_Gain_n; + real_T Gain1_Gain_lk; + real_T Vm_currentms_Value_b; + real_T Gain_Gain_jq; + real_T uDLookupTable_tableData_a[7]; + real_T uDLookupTable_bp01Data_m[7]; + real_T Saturation3_UpperSat_e; + real_T Saturation3_LowerSat_k; + real_T Gain5_Gain_m; + real_T Bias_Bias_m; + real_T Theta_max3_Value; + real_T Gain3_Gain_g; + real_T Saturation2_UpperSat; + real_T Saturation2_LowerSat; + real_T Loaddemand2_tableData[3]; + real_T Loaddemand2_bp01Data[3]; + real_T PLUT_tableData_g[2]; + real_T PLUT_bp01Data_e[2]; + real_T DLUT_tableData_l[2]; + real_T DLUT_bp01Data_hw[2]; + real_T SaturationV_dot_UpperSat_j2; + real_T SaturationV_dot_LowerSat_n; + real_T Gain_Gain_l0; + real_T SaturationSpoilers_UpperSat_m; + real_T SaturationSpoilers_LowerSat_d; + real_T Saturation_UpperSat_a; + real_T Saturation_LowerSat_k; + real_T Switch_Threshold; + real_T Gain_Gain_cy; + real_T Saturation_UpperSat_l; + real_T Saturation_LowerSat_kp; + real_T Constant_Value_o1; + real_T Constant2_Value_k; + real_T uDLookupTable_tableData_e5[25]; + real_T uDLookupTable_bp01Data_l[5]; + real_T uDLookupTable_bp02Data[5]; + real_T Saturation3_UpperSat_l; + real_T Saturation3_LowerSat_h; + real_T PitchRateDemand_tableData[3]; + real_T PitchRateDemand_bp01Data[3]; + real_T Gain3_Gain_e; + real_T Gain_Gain_pt; + real_T Gain1_Gain_d; + real_T Gain1_Gain_a; + real_T Gain5_Gain_h; + real_T Gain4_Gain; + real_T Gain6_Gain_g; + real_T Constant_Value_jk; + real_T Saturation_UpperSat_m; + real_T Saturation_LowerSat_b; + real_T Constant_Value_h; + real_T Saturation_UpperSat_kp; + real_T Saturation_LowerSat_a4; + uint32_T uDLookupTable_maxIndex[2]; + uint8_T ManualSwitch_CurrentSetting; + uint8_T ManualSwitch1_CurrentSetting; + }; + + void init(); + PitchNormalLaw(PitchNormalLaw const&) = delete; + PitchNormalLaw& operator= (PitchNormalLaw const&) & = delete; + PitchNormalLaw(PitchNormalLaw &&) = delete; + PitchNormalLaw& operator= (PitchNormalLaw &&) = delete; + void step(const real_T *rtu_In_time_dt, const real_T *rtu_In_nz_g, const real_T *rtu_In_Theta_deg, const real_T + *rtu_In_Phi_deg, const real_T *rtu_In_qk_deg_s, const real_T *rtu_In_qk_dot_deg_s2, const real_T + *rtu_In_eta_deg, const real_T *rtu_In_eta_trim_deg, const real_T *rtu_In_alpha_deg, const real_T + *rtu_In_V_ias_kn, const real_T *rtu_In_V_tas_kn, const real_T *rtu_In_H_radio_ft, const real_T + *rtu_In_flaps_handle_index, const real_T *rtu_In_spoilers_left_pos, const real_T *rtu_In_spoilers_right_pos, + const real_T *rtu_In_thrust_lever_1_pos, const real_T *rtu_In_thrust_lever_2_pos, const boolean_T + *rtu_In_tailstrike_protection_on, const real_T *rtu_In_VLS_kn, const real_T *rtu_In_delta_eta_pos, const + boolean_T *rtu_In_on_ground, const real_T *rtu_In_in_flight, const boolean_T *rtu_In_tracking_mode_on, const + boolean_T *rtu_In_high_aoa_prot_active, const boolean_T *rtu_In_high_speed_prot_active, const real_T + *rtu_In_alpha_prot, const real_T *rtu_In_alpha_max, const real_T *rtu_In_high_speed_prot_high_kn, const + real_T *rtu_In_high_speed_prot_low_kn, const real_T *rtu_In_ap_theta_c_deg, const boolean_T + *rtu_In_any_ap_engaged, real_T *rty_Out_eta_deg, real_T *rty_Out_eta_trim_dot_deg_s, real_T + *rty_Out_eta_trim_limit_lo, real_T *rty_Out_eta_trim_limit_up); + void reset(); + PitchNormalLaw(); + ~PitchNormalLaw(); + private: + BlockIO_PitchNormalLaw_T PitchNormalLaw_B; + D_Work_PitchNormalLaw_T PitchNormalLaw_DWork; + static Parameters_PitchNormalLaw_T PitchNormalLaw_rtP; + static void PitchNormalLaw_LagFilter_Reset(rtDW_LagFilter_PitchNormalLaw_T *localDW); + static void PitchNormalLaw_LagFilter(const real_T *rtu_U, real_T rtu_C1, const real_T *rtu_dt, real_T *rty_Y, + rtDW_LagFilter_PitchNormalLaw_T *localDW); + static void PitchNormalLaw_RateLimiter_Reset(rtDW_RateLimiter_PitchNormalLaw_T *localDW); + static void PitchNormalLaw_RateLimiter(real_T rtu_u, real_T rtu_up, real_T rtu_lo, const real_T *rtu_Ts, real_T + rtu_init, real_T *rty_Y, rtDW_RateLimiter_PitchNormalLaw_T *localDW); + static void PitchNormalLaw_eta_trim_limit_lofreeze_Reset(rtDW_eta_trim_limit_lofreeze_PitchNormalLaw_T *localDW); + static void PitchNormalLaw_eta_trim_limit_lofreeze(const real_T *rtu_eta_trim, const boolean_T *rtu_trigger, real_T + *rty_y, rtDW_eta_trim_limit_lofreeze_PitchNormalLaw_T *localDW); + static void PitchNormalLaw_LagFilter_i_Reset(rtDW_LagFilter_PitchNormalLaw_d_T *localDW); + static void PitchNormalLaw_LagFilter_n(real_T rtu_U, real_T rtu_C1, const real_T *rtu_dt, real_T *rty_Y, + rtDW_LagFilter_PitchNormalLaw_d_T *localDW); + static void PitchNormalLaw_WashoutFilter_Reset(rtDW_WashoutFilter_PitchNormalLaw_T *localDW); + static void PitchNormalLaw_WashoutFilter(real_T rtu_U, real_T rtu_C1, const real_T *rtu_dt, real_T *rty_Y, + rtDW_WashoutFilter_PitchNormalLaw_T *localDW); + static void PitchNormalLaw_RateLimiter_l_Reset(rtDW_RateLimiter_PitchNormalLaw_o_T *localDW); + static void PitchNormalLaw_RateLimiter_c(const real_T *rtu_u, real_T rtu_up, real_T rtu_lo, const real_T *rtu_Ts, + real_T rtu_init, real_T *rty_Y, rtDW_RateLimiter_PitchNormalLaw_o_T *localDW); + static void PitchNormalLaw_VoterAttitudeProtection(real_T rtu_input, real_T rtu_input_l, real_T rtu_input_o, real_T + *rty_vote); +}; + +extern PitchNormalLaw::Parameters_PitchNormalLaw_T PitchNormalLaw_rtP; + +#endif + diff --git a/hdw-a339x/src/wasm/fbw_a320/src/model/PitchNormalLaw_private.h b/hdw-a339x/src/wasm/fbw_a320/src/model/PitchNormalLaw_private.h new file mode 100644 index 000000000..be77f4434 --- /dev/null +++ b/hdw-a339x/src/wasm/fbw_a320/src/model/PitchNormalLaw_private.h @@ -0,0 +1,6 @@ +#ifndef RTW_HEADER_PitchNormalLaw_private_h_ +#define RTW_HEADER_PitchNormalLaw_private_h_ +#include "rtwtypes.h" +#include "PitchNormalLaw_types.h" +#endif + diff --git a/hdw-a339x/src/wasm/fbw_a320/src/model/SecComputer.cpp b/hdw-a339x/src/wasm/fbw_a320/src/model/SecComputer.cpp new file mode 100644 index 000000000..f098d3449 --- /dev/null +++ b/hdw-a339x/src/wasm/fbw_a320/src/model/SecComputer.cpp @@ -0,0 +1,1439 @@ +#include "SecComputer.h" +#include "SecComputer_types.h" +#include "rtwtypes.h" +#include +#include "look1_binlxpw.h" +#include "LateralDirectLaw.h" +#include "PitchAlternateLaw.h" +#include "PitchDirectLaw.h" + +const uint8_T SecComputer_IN_InAir{ 1U }; + +const uint8_T SecComputer_IN_NO_ACTIVE_CHILD{ 0U }; + +const uint8_T SecComputer_IN_OnGround{ 2U }; + +const uint8_T SecComputer_IN_Flight{ 1U }; + +const uint8_T SecComputer_IN_FlightToGroundTransition{ 2U }; + +const uint8_T SecComputer_IN_Ground{ 3U }; + +const real_T SecComputer_RGND{ 0.0 }; + +void SecComputer::SecComputer_MATLABFunction(const base_arinc_429 *rtu_u, real_T rtu_bit, uint32_T *rty_y) +{ + real32_T tmp; + uint32_T a; + tmp = std::round(rtu_u->Data); + if (tmp < 4.2949673E+9F) { + if (tmp >= 0.0F) { + a = static_cast(tmp); + } else { + a = 0U; + } + } else { + a = MAX_uint32_T; + } + + if (-(rtu_bit - 1.0) >= 0.0) { + if (-(rtu_bit - 1.0) <= 31.0) { + a <<= static_cast(-(rtu_bit - 1.0)); + } else { + a = 0U; + } + } else if (-(rtu_bit - 1.0) >= -31.0) { + a >>= static_cast(rtu_bit - 1.0); + } else { + a = 0U; + } + + *rty_y = a & 1U; +} + +void SecComputer::SecComputer_RateLimiter_Reset(rtDW_RateLimiter_SecComputer_T *localDW) +{ + localDW->pY_not_empty = false; +} + +void SecComputer::SecComputer_RateLimiter(real_T rtu_u, real_T rtu_up, real_T rtu_lo, real_T rtu_Ts, real_T rtu_init, + real_T *rty_Y, rtDW_RateLimiter_SecComputer_T *localDW) +{ + if (!localDW->pY_not_empty) { + localDW->pY = rtu_init; + localDW->pY_not_empty = true; + } + + localDW->pY += std::fmax(std::fmin(rtu_u - localDW->pY, std::abs(rtu_up) * rtu_Ts), -std::abs(rtu_lo) * rtu_Ts); + *rty_Y = localDW->pY; +} + +void SecComputer::SecComputer_RateLimiter_n_Reset(rtDW_RateLimiter_SecComputer_m_T *localDW) +{ + localDW->pY_not_empty = false; +} + +void SecComputer::SecComputer_RateLimiter_b(real_T rtu_u, real_T rtu_up, real_T rtu_lo, real_T rtu_Ts, real_T rtu_init, + boolean_T rtu_reset, real_T *rty_Y, rtDW_RateLimiter_SecComputer_m_T *localDW) +{ + if ((!localDW->pY_not_empty) || rtu_reset) { + localDW->pY = rtu_init; + localDW->pY_not_empty = true; + } + + if (rtu_reset) { + *rty_Y = rtu_init; + } else { + *rty_Y = std::fmax(std::fmin(rtu_u - localDW->pY, std::abs(rtu_up) * rtu_Ts), -std::abs(rtu_lo) * rtu_Ts) + + localDW->pY; + } + + localDW->pY = *rty_Y; +} + +void SecComputer::SecComputer_MATLABFunction_g_Reset(rtDW_MATLABFunction_SecComputer_l_T *localDW) +{ + localDW->previousInput_not_empty = false; +} + +void SecComputer::SecComputer_MATLABFunction_e(boolean_T rtu_u, boolean_T rtu_isRisingEdge, boolean_T *rty_y, + rtDW_MATLABFunction_SecComputer_l_T *localDW) +{ + if (!localDW->previousInput_not_empty) { + localDW->previousInput = rtu_isRisingEdge; + localDW->previousInput_not_empty = true; + } + + if (rtu_isRisingEdge) { + *rty_y = (rtu_u && (!localDW->previousInput)); + } else { + *rty_y = ((!rtu_u) && localDW->previousInput); + } + + localDW->previousInput = rtu_u; +} + +void SecComputer::SecComputer_MATLABFunction_e_Reset(rtDW_MATLABFunction_SecComputer_o_T *localDW) +{ + localDW->output = false; + localDW->timeSinceCondition = 0.0; +} + +void SecComputer::SecComputer_MATLABFunction_n(boolean_T rtu_u, real_T rtu_Ts, boolean_T rtu_isRisingEdge, real_T + rtu_timeDelay, boolean_T *rty_y, rtDW_MATLABFunction_SecComputer_o_T *localDW) +{ + if (rtu_u == rtu_isRisingEdge) { + localDW->timeSinceCondition += rtu_Ts; + if (localDW->timeSinceCondition >= rtu_timeDelay) { + localDW->output = rtu_u; + } + } else { + localDW->timeSinceCondition = 0.0; + localDW->output = rtu_u; + } + + *rty_y = localDW->output; +} + +void SecComputer::SecComputer_MATLABFunction_l(const base_arinc_429 *rtu_u, boolean_T *rty_y) +{ + *rty_y = (rtu_u->SSM != static_cast(SignStatusMatrix::FailureWarning)); +} + +void SecComputer::SecComputer_MATLABFunction_c(const boolean_T rtu_u[19], real32_T *rty_y) +{ + uint32_T out; + out = 0U; + for (int32_T i{0}; i < 19; i++) { + out |= static_cast(rtu_u[i]) << (i + 10); + } + + *rty_y = static_cast(out); +} + +void SecComputer::step() +{ + real_T rtb_eta_deg; + real_T rtb_eta_trim_dot_deg_s; + real_T rtb_eta_trim_limit_lo; + real_T rtb_eta_trim_limit_up; + base_arinc_429 rtb_Switch8; + real_T pair1SpdBrkCommand; + real_T rollCommand; + real_T rtb_BusAssignment_f_logic_ir_computation_data_n_z_g; + real_T rtb_BusAssignment_f_logic_ir_computation_data_theta_dot_deg_s; + real_T rtb_BusConversion_InsertedFor_BusAssignment_at_inport_8_BusCreator1_V_tas_kn; + real_T rtb_BusConversion_InsertedFor_BusAssignment_at_inport_8_BusCreator1_mach; + real_T rtb_Switch3; + real_T rtb_Switch5; + real_T rtb_Switch5_tmp_tmp; + real_T rtb_Switch6_m; + real_T rtb_eta_trim_limit_lo_d; + real_T rtb_handleIndex; + real_T rtb_zeta_deg; + real_T u0_0; + uint32_T rtb_DataTypeConversion1; + uint32_T rtb_Switch7_c; + uint32_T rtb_Switch9_c; + uint32_T rtb_y; + uint32_T rtb_y_d; + uint32_T rtb_y_m; + boolean_T rtb_VectorConcatenate[19]; + boolean_T rtb_AND1_h; + boolean_T rtb_AND4_a; + boolean_T rtb_NOT_bl; + boolean_T rtb_OR1; + boolean_T rtb_y_b; + boolean_T rtb_y_gp; + boolean_T rtb_y_k; + boolean_T rtb_y_kz; + boolean_T rtb_y_l; + boolean_T rtb_y_p; + boolean_T rtb_y_p1; + if (SecComputer_U.in.sim_data.computer_running) { + real_T pair1RollCommand; + real_T u0; + real32_T rtb_V_ias; + real32_T rtb_V_tas; + real32_T rtb_alpha; + real32_T rtb_mach; + real32_T rtb_n_x; + real32_T rtb_n_y; + real32_T rtb_n_z; + real32_T rtb_phi; + real32_T rtb_phi_dot; + real32_T rtb_q; + real32_T rtb_r; + real32_T rtb_theta; + real32_T rtb_theta_dot; + boolean_T abnormalCondition; + boolean_T canEngageInPitch; + boolean_T hasPriorityInPitch; + boolean_T leftElevatorAvail; + boolean_T rightElevatorAvail; + boolean_T rtb_AND2_j; + boolean_T rtb_AND2_p; + boolean_T rtb_NOT2_b; + boolean_T rtb_NOT_g; + boolean_T rtb_OR; + boolean_T rtb_OR3; + boolean_T rtb_OR6; + boolean_T rtb_doubleAdrFault; + boolean_T rtb_doubleIrFault; + boolean_T rtb_isEngagedInPitch; + boolean_T rtb_isEngagedInRoll; + boolean_T rtb_singleAdrFault; + boolean_T rtb_singleIrFault; + boolean_T rtb_thsAvail; + boolean_T spoilerPair1SupplyAvail; + boolean_T spoiler_pair_2_avail; + pitch_efcs_law rtb_activePitchLaw; + pitch_efcs_law rtb_pitchLawCapability; + if (!SecComputer_DWork.Runtime_MODE) { + SecComputer_DWork.Delay_DSTATE_c = SecComputer_P.Delay_InitialCondition_c; + SecComputer_DWork.Delay1_DSTATE = SecComputer_P.Delay1_InitialCondition; + SecComputer_DWork.Memory_PreviousInput = SecComputer_P.SRFlipFlop_initial_condition; + SecComputer_DWork.Memory_PreviousInput_f = SecComputer_P.SRFlipFlop_initial_condition_c; + SecComputer_DWork.Memory_PreviousInput_n = SecComputer_P.SRFlipFlop_initial_condition_k; + SecComputer_DWork.Delay1_DSTATE_i = SecComputer_P.Delay1_InitialCondition_l; + SecComputer_DWork.Delay_DSTATE_n = SecComputer_P.Delay_InitialCondition_j; + SecComputer_DWork.Delay2_DSTATE = SecComputer_P.Delay2_InitialCondition; + SecComputer_DWork.Delay_DSTATE = SecComputer_P.Delay_InitialCondition; + SecComputer_DWork.icLoad = true; + SecComputer_MATLABFunction_e_Reset(&SecComputer_DWork.sf_MATLABFunction_jk); + SecComputer_MATLABFunction_e_Reset(&SecComputer_DWork.sf_MATLABFunction_dw); + SecComputer_DWork.is_active_c8_SecComputer = 0U; + SecComputer_DWork.is_c8_SecComputer = SecComputer_IN_NO_ACTIVE_CHILD; + SecComputer_DWork.is_active_c30_SecComputer = 0U; + SecComputer_DWork.is_c30_SecComputer = SecComputer_IN_NO_ACTIVE_CHILD; + SecComputer_DWork.on_ground_time = 0.0; + SecComputer_B.in_flight = 0.0; + SecComputer_MATLABFunction_e_Reset(&SecComputer_DWork.sf_MATLABFunction_ndv); + SecComputer_MATLABFunction_e_Reset(&SecComputer_DWork.sf_MATLABFunction_gf); + SecComputer_MATLABFunction_e_Reset(&SecComputer_DWork.sf_MATLABFunction_h); + SecComputer_MATLABFunction_g_Reset(&SecComputer_DWork.sf_MATLABFunction_g4b); + SecComputer_MATLABFunction_g_Reset(&SecComputer_DWork.sf_MATLABFunction_nu); + SecComputer_DWork.pLeftStickDisabled = false; + SecComputer_DWork.pRightStickDisabled = false; + SecComputer_MATLABFunction_e_Reset(&SecComputer_DWork.sf_MATLABFunction_j2); + SecComputer_MATLABFunction_e_Reset(&SecComputer_DWork.sf_MATLABFunction_g24); + SecComputer_MATLABFunction_e_Reset(&SecComputer_DWork.sf_MATLABFunction_k4); + SecComputer_DWork.abnormalConditionWasActive = false; + SecComputer_MATLABFunction_g_Reset(&SecComputer_DWork.sf_MATLABFunction_b4); + SecComputer_MATLABFunction_e_Reset(&SecComputer_DWork.sf_MATLABFunction_fh); + SecComputer_MATLABFunction_g_Reset(&SecComputer_DWork.sf_MATLABFunction_nd); + SecComputer_MATLABFunction_g_Reset(&SecComputer_DWork.sf_MATLABFunction_n); + SecComputer_MATLABFunction_g_Reset(&SecComputer_DWork.sf_MATLABFunction_a); + SecComputer_MATLABFunction_g_Reset(&SecComputer_DWork.sf_MATLABFunction_e3); + SecComputer_RateLimiter_Reset(&SecComputer_DWork.sf_RateLimiter_c); + SecComputer_RateLimiter_Reset(&SecComputer_DWork.sf_RateLimiter); + LawMDLOBJ1.reset(); + SecComputer_RateLimiter_n_Reset(&SecComputer_DWork.sf_RateLimiter_b); + SecComputer_RateLimiter_n_Reset(&SecComputer_DWork.sf_RateLimiter_a); + SecComputer_RateLimiter_n_Reset(&SecComputer_DWork.sf_RateLimiter_k); + SecComputer_RateLimiter_n_Reset(&SecComputer_DWork.sf_RateLimiter_b4); + LawMDLOBJ2.reset(); + LawMDLOBJ3.reset(); + SecComputer_MATLABFunction_e_Reset(&SecComputer_DWork.sf_MATLABFunction_i); + SecComputer_DWork.Runtime_MODE = true; + } + + rtb_OR1 = ((SecComputer_U.in.bus_inputs.adr_1_bus.mach.SSM == static_cast(SignStatusMatrix::FailureWarning)) + || (SecComputer_U.in.bus_inputs.adr_1_bus.airspeed_computed_kn.SSM == static_cast + (SignStatusMatrix::FailureWarning)) || (SecComputer_U.in.bus_inputs.adr_1_bus.airspeed_true_kn.SSM == + static_cast(SignStatusMatrix::FailureWarning)) || + (SecComputer_U.in.bus_inputs.adr_1_bus.aoa_corrected_deg.SSM == static_cast(SignStatusMatrix:: + FailureWarning)) || SecComputer_P.Constant2_Value_c || SecComputer_P.Constant2_Value_c); + rtb_OR3 = ((SecComputer_U.in.bus_inputs.adr_2_bus.mach.SSM == static_cast(SignStatusMatrix::FailureWarning)) + || (SecComputer_U.in.bus_inputs.adr_2_bus.airspeed_computed_kn.SSM == static_cast + (SignStatusMatrix::FailureWarning)) || (SecComputer_U.in.bus_inputs.adr_2_bus.airspeed_true_kn.SSM == + static_cast(SignStatusMatrix::FailureWarning)) || + (SecComputer_U.in.bus_inputs.adr_2_bus.aoa_corrected_deg.SSM == static_cast(SignStatusMatrix:: + FailureWarning)) || SecComputer_P.Constant2_Value_c || SecComputer_P.Constant2_Value_c); + rtb_singleAdrFault = (rtb_OR1 || rtb_OR3); + rtb_doubleAdrFault = (rtb_OR1 && rtb_OR3); + rtb_OR = ((SecComputer_U.in.bus_inputs.ir_1_bus.pitch_angle_deg.SSM != static_cast(SignStatusMatrix:: + NormalOperation)) || (SecComputer_U.in.bus_inputs.ir_1_bus.roll_angle_deg.SSM != static_cast + (SignStatusMatrix::NormalOperation)) || (SecComputer_U.in.bus_inputs.ir_1_bus.body_yaw_rate_deg_s.SSM != + static_cast(SignStatusMatrix::NormalOperation)) || + (SecComputer_U.in.bus_inputs.ir_1_bus.body_long_accel_g.SSM != static_cast(SignStatusMatrix:: + NormalOperation)) || (SecComputer_U.in.bus_inputs.ir_1_bus.body_lat_accel_g.SSM != static_cast + (SignStatusMatrix::NormalOperation)) || (SecComputer_U.in.bus_inputs.ir_1_bus.body_normal_accel_g.SSM != + static_cast(SignStatusMatrix::NormalOperation)) || + (SecComputer_U.in.bus_inputs.ir_1_bus.pitch_att_rate_deg_s.SSM != static_cast(SignStatusMatrix:: + NormalOperation)) || (SecComputer_U.in.bus_inputs.ir_1_bus.roll_att_rate_deg_s.SSM != + static_cast(SignStatusMatrix::NormalOperation)) || SecComputer_P.Constant_Value_l); + rtb_OR6 = ((SecComputer_U.in.bus_inputs.ir_2_bus.pitch_angle_deg.SSM != static_cast(SignStatusMatrix:: + NormalOperation)) || (SecComputer_U.in.bus_inputs.ir_2_bus.roll_angle_deg.SSM != static_cast + (SignStatusMatrix::NormalOperation)) || (SecComputer_U.in.bus_inputs.ir_2_bus.body_yaw_rate_deg_s.SSM != + static_cast(SignStatusMatrix::NormalOperation)) || + (SecComputer_U.in.bus_inputs.ir_2_bus.body_long_accel_g.SSM != static_cast(SignStatusMatrix:: + NormalOperation)) || (SecComputer_U.in.bus_inputs.ir_2_bus.body_lat_accel_g.SSM != static_cast + (SignStatusMatrix::NormalOperation)) || (SecComputer_U.in.bus_inputs.ir_2_bus.body_normal_accel_g.SSM != + static_cast(SignStatusMatrix::NormalOperation)) || + (SecComputer_U.in.bus_inputs.ir_2_bus.pitch_att_rate_deg_s.SSM != static_cast(SignStatusMatrix:: + NormalOperation)) || (SecComputer_U.in.bus_inputs.ir_2_bus.roll_att_rate_deg_s.SSM != static_cast + (SignStatusMatrix::NormalOperation)) || SecComputer_P.Constant_Value_l); + rtb_singleIrFault = (rtb_OR || rtb_OR6); + rtb_doubleIrFault = (rtb_OR && rtb_OR6); + rtb_y_gp = !rtb_OR3; + if ((!rtb_OR1) && rtb_y_gp) { + rtb_V_ias = (SecComputer_U.in.bus_inputs.adr_2_bus.airspeed_computed_kn.Data + + SecComputer_U.in.bus_inputs.adr_2_bus.airspeed_computed_kn.Data) / 2.0F; + rtb_V_tas = (SecComputer_U.in.bus_inputs.adr_2_bus.airspeed_true_kn.Data + + SecComputer_U.in.bus_inputs.adr_2_bus.airspeed_true_kn.Data) / 2.0F; + rtb_mach = (SecComputer_U.in.bus_inputs.adr_2_bus.mach.Data + SecComputer_U.in.bus_inputs.adr_2_bus.mach.Data) / + 2.0F; + rtb_alpha = (SecComputer_U.in.bus_inputs.adr_2_bus.aoa_corrected_deg.Data + + SecComputer_U.in.bus_inputs.adr_2_bus.aoa_corrected_deg.Data) / 2.0F; + } else if ((!rtb_OR1) && rtb_OR3) { + rtb_V_ias = SecComputer_U.in.bus_inputs.adr_1_bus.airspeed_computed_kn.Data; + rtb_V_tas = SecComputer_U.in.bus_inputs.adr_1_bus.airspeed_true_kn.Data; + rtb_mach = SecComputer_U.in.bus_inputs.adr_1_bus.mach.Data; + rtb_alpha = SecComputer_U.in.bus_inputs.adr_1_bus.aoa_corrected_deg.Data; + } else if (rtb_OR1 && rtb_y_gp) { + rtb_V_ias = SecComputer_U.in.bus_inputs.adr_2_bus.airspeed_computed_kn.Data; + rtb_V_tas = SecComputer_U.in.bus_inputs.adr_2_bus.airspeed_true_kn.Data; + rtb_mach = SecComputer_U.in.bus_inputs.adr_2_bus.mach.Data; + rtb_alpha = SecComputer_U.in.bus_inputs.adr_2_bus.aoa_corrected_deg.Data; + } else { + rtb_V_ias = 0.0F; + rtb_V_tas = 0.0F; + rtb_mach = 0.0F; + rtb_alpha = 0.0F; + } + + rtb_eta_trim_limit_lo_d = rtb_V_ias; + rtb_BusConversion_InsertedFor_BusAssignment_at_inport_8_BusCreator1_V_tas_kn = rtb_V_tas; + rtb_BusConversion_InsertedFor_BusAssignment_at_inport_8_BusCreator1_mach = rtb_mach; + rtb_y_gp = !rtb_OR; + rtb_OR1 = !rtb_OR6; + if (rtb_y_gp && rtb_OR1) { + rtb_theta = (SecComputer_U.in.bus_inputs.ir_1_bus.pitch_angle_deg.Data + + SecComputer_U.in.bus_inputs.ir_2_bus.pitch_angle_deg.Data) / 2.0F; + rtb_phi = (SecComputer_U.in.bus_inputs.ir_1_bus.roll_angle_deg.Data + + SecComputer_U.in.bus_inputs.ir_2_bus.roll_angle_deg.Data) / 2.0F; + rtb_q = (SecComputer_U.in.bus_inputs.ir_1_bus.body_pitch_rate_deg_s.Data + + SecComputer_U.in.bus_inputs.ir_2_bus.body_pitch_rate_deg_s.Data) / 2.0F; + rtb_r = (SecComputer_U.in.bus_inputs.ir_1_bus.body_yaw_rate_deg_s.Data + + SecComputer_U.in.bus_inputs.ir_2_bus.body_yaw_rate_deg_s.Data) / 2.0F; + rtb_n_x = (SecComputer_U.in.bus_inputs.ir_1_bus.body_long_accel_g.Data + + SecComputer_U.in.bus_inputs.ir_2_bus.body_long_accel_g.Data) / 2.0F; + rtb_n_y = (SecComputer_U.in.bus_inputs.ir_1_bus.body_lat_accel_g.Data + + SecComputer_U.in.bus_inputs.ir_2_bus.body_lat_accel_g.Data) / 2.0F; + rtb_n_z = (SecComputer_U.in.bus_inputs.ir_1_bus.body_normal_accel_g.Data + + SecComputer_U.in.bus_inputs.ir_2_bus.body_normal_accel_g.Data) / 2.0F; + rtb_theta_dot = (SecComputer_U.in.bus_inputs.ir_1_bus.pitch_att_rate_deg_s.Data + + SecComputer_U.in.bus_inputs.ir_2_bus.pitch_att_rate_deg_s.Data) / 2.0F; + rtb_phi_dot = (SecComputer_U.in.bus_inputs.ir_1_bus.roll_att_rate_deg_s.Data + + SecComputer_U.in.bus_inputs.ir_2_bus.roll_att_rate_deg_s.Data) / 2.0F; + } else if (rtb_y_gp && rtb_OR6) { + rtb_theta = SecComputer_U.in.bus_inputs.ir_1_bus.pitch_angle_deg.Data; + rtb_phi = SecComputer_U.in.bus_inputs.ir_1_bus.roll_angle_deg.Data; + rtb_q = SecComputer_U.in.bus_inputs.ir_1_bus.body_pitch_rate_deg_s.Data; + rtb_r = SecComputer_U.in.bus_inputs.ir_1_bus.body_yaw_rate_deg_s.Data; + rtb_n_x = SecComputer_U.in.bus_inputs.ir_1_bus.body_long_accel_g.Data; + rtb_n_y = SecComputer_U.in.bus_inputs.ir_1_bus.body_lat_accel_g.Data; + rtb_n_z = SecComputer_U.in.bus_inputs.ir_1_bus.body_normal_accel_g.Data; + rtb_theta_dot = SecComputer_U.in.bus_inputs.ir_1_bus.pitch_att_rate_deg_s.Data; + rtb_phi_dot = SecComputer_U.in.bus_inputs.ir_1_bus.roll_att_rate_deg_s.Data; + } else if (rtb_OR && rtb_OR1) { + rtb_theta = SecComputer_U.in.bus_inputs.ir_2_bus.pitch_angle_deg.Data; + rtb_phi = SecComputer_U.in.bus_inputs.ir_2_bus.roll_angle_deg.Data; + rtb_q = SecComputer_U.in.bus_inputs.ir_2_bus.body_pitch_rate_deg_s.Data; + rtb_r = SecComputer_U.in.bus_inputs.ir_2_bus.body_yaw_rate_deg_s.Data; + rtb_n_x = SecComputer_U.in.bus_inputs.ir_2_bus.body_long_accel_g.Data; + rtb_n_y = SecComputer_U.in.bus_inputs.ir_2_bus.body_lat_accel_g.Data; + rtb_n_z = SecComputer_U.in.bus_inputs.ir_2_bus.body_normal_accel_g.Data; + rtb_theta_dot = SecComputer_U.in.bus_inputs.ir_2_bus.pitch_att_rate_deg_s.Data; + rtb_phi_dot = SecComputer_U.in.bus_inputs.ir_2_bus.roll_att_rate_deg_s.Data; + } else { + rtb_theta = 0.0F; + rtb_phi = 0.0F; + rtb_q = 0.0F; + rtb_r = 0.0F; + rtb_n_x = 0.0F; + rtb_n_y = 0.0F; + rtb_n_z = 0.0F; + rtb_theta_dot = 0.0F; + rtb_phi_dot = 0.0F; + } + + rtb_Switch5_tmp_tmp = rtb_theta; + rtb_Switch5 = rtb_theta; + rtb_Switch6_m = rtb_phi; + rtb_handleIndex = rtb_n_x; + SecComputer_MATLABFunction_l(&SecComputer_U.in.bus_inputs.lgciu_2_bus.discrete_word_3, &rtb_y_p); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.lgciu_2_bus.discrete_word_3, + SecComputer_P.BitfromLabel13_bit, &rtb_Switch7_c); + rtb_OR1 = (rtb_Switch7_c != 0U); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.lgciu_2_bus.discrete_word_3, + SecComputer_P.BitfromLabel12_bit, &rtb_Switch7_c); + rtb_y_l = (rtb_Switch7_c != 0U); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.lgciu_2_bus.discrete_word_3, + SecComputer_P.BitfromLabel10_bit, &rtb_Switch7_c); + rtb_OR1 = (rtb_OR1 || rtb_y_l || (rtb_Switch7_c != 0U)); + rtb_y_kz = (rtb_y_p && rtb_OR1); + SecComputer_MATLABFunction_l(&SecComputer_U.in.bus_inputs.lgciu_1_bus.discrete_word_3, &rtb_NOT_bl); + rtb_OR = (rtb_y_kz && (!rtb_NOT_bl)); + SecComputer_MATLABFunction_l(&SecComputer_U.in.bus_inputs.lgciu_1_bus.discrete_word_3, &rtb_y_gp); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.lgciu_1_bus.discrete_word_3, SecComputer_P.BitfromLabel5_bit, + &rtb_Switch7_c); + rtb_y_l = (rtb_Switch7_c != 0U); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.lgciu_1_bus.discrete_word_3, SecComputer_P.BitfromLabel4_bit, + &rtb_Switch7_c); + rtb_OR6 = (rtb_Switch7_c != 0U); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.lgciu_1_bus.discrete_word_3, SecComputer_P.BitfromLabel2_bit, + &rtb_Switch7_c); + rtb_y_l = (rtb_y_l || rtb_OR6 || (rtb_Switch7_c != 0U)); + rtb_AND4_a = (rtb_y_gp && rtb_y_l); + SecComputer_MATLABFunction_l(&SecComputer_U.in.bus_inputs.lgciu_2_bus.discrete_word_3, &rtb_NOT_bl); + rtb_OR = (rtb_OR || (rtb_AND4_a && (!rtb_NOT_bl)) || (rtb_y_kz && rtb_AND4_a)); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.lgciu_1_bus.discrete_word_3, + SecComputer_P.BitfromLabel15_bit, &rtb_Switch7_c); + rtb_OR6 = (rtb_Switch7_c != 0U); + SecComputer_MATLABFunction_l(&SecComputer_U.in.bus_inputs.lgciu_1_bus.discrete_word_3, &rtb_y_k); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.lgciu_1_bus.discrete_word_3, + SecComputer_P.BitfromLabel14_bit, &rtb_Switch7_c); + rtb_y_kz = (rtb_Switch7_c != 0U); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.lgciu_1_bus.discrete_word_3, + SecComputer_P.BitfromLabel11_bit, &rtb_Switch7_c); + rtb_AND1_h = (rtb_Switch7_c != 0U); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.lgciu_1_bus.discrete_word_3, SecComputer_P.BitfromLabel8_bit, + &rtb_Switch7_c); + rtb_OR3 = ((!rtb_y_kz) && (!rtb_AND1_h) && (rtb_Switch7_c == 0U)); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.lgciu_2_bus.discrete_word_3, + SecComputer_P.BitfromLabel16_bit, &rtb_Switch7_c); + rtb_NOT_bl = (rtb_Switch7_c != 0U); + SecComputer_MATLABFunction_n((rtb_Switch7_c != 0U) && rtb_y_p && rtb_OR1, SecComputer_U.in.time.dt, + SecComputer_P.ConfirmNode_isRisingEdge, SecComputer_P.ConfirmNode_timeDelay, &rtb_OR1, + &SecComputer_DWork.sf_MATLABFunction_jk); + rtb_OR1 = (rtb_OR6 && rtb_y_k && rtb_OR3 && rtb_OR1); + SecComputer_MATLABFunction_l(&SecComputer_U.in.bus_inputs.lgciu_1_bus.discrete_word_3, &rtb_AND1_h); + SecComputer_MATLABFunction_l(&SecComputer_U.in.bus_inputs.lgciu_2_bus.discrete_word_3, &rtb_NOT_bl); + rtb_OR3 = ((!rtb_AND1_h) && (!rtb_NOT_bl)); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.lgciu_2_bus.discrete_word_3, SecComputer_P.BitfromLabel7_bit, + &rtb_Switch7_c); + rtb_NOT_bl = (rtb_Switch7_c != 0U); + SecComputer_MATLABFunction_l(&SecComputer_U.in.bus_inputs.lgciu_2_bus.discrete_word_3, &rtb_y_k); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.lgciu_2_bus.discrete_word_3, SecComputer_P.BitfromLabel6_bit, + &rtb_Switch7_c); + rtb_AND1_h = (rtb_Switch7_c != 0U); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.lgciu_2_bus.discrete_word_3, SecComputer_P.BitfromLabel3_bit, + &rtb_Switch7_c); + rtb_OR6 = (rtb_Switch7_c != 0U); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.lgciu_2_bus.discrete_word_3, SecComputer_P.BitfromLabel1_bit, + &rtb_Switch7_c); + rtb_OR6 = (rtb_NOT_bl && rtb_y_k && ((!rtb_AND1_h) && (!rtb_OR6) && (rtb_Switch7_c == 0U))); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.lgciu_1_bus.discrete_word_3, SecComputer_P.BitfromLabel9_bit, + &rtb_Switch7_c); + rtb_NOT_bl = (rtb_Switch7_c != 0U); + SecComputer_MATLABFunction_n((rtb_Switch7_c != 0U) && rtb_y_gp && rtb_y_l, SecComputer_U.in.time.dt, + SecComputer_P.ConfirmNode1_isRisingEdge, SecComputer_P.ConfirmNode1_timeDelay, &rtb_NOT_bl, + &SecComputer_DWork.sf_MATLABFunction_dw); + rtb_OR6 = (rtb_OR1 || rtb_OR3 || (rtb_OR6 && rtb_NOT_bl)); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.lgciu_1_bus.discrete_word_2, + SecComputer_P.BitfromLabel4_bit_c, &rtb_Switch7_c); + rtb_NOT_bl = (rtb_Switch7_c != 0U); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.lgciu_2_bus.discrete_word_2, + SecComputer_P.BitfromLabel6_bit_l, &rtb_Switch7_c); + rtb_OR1 = (rtb_NOT_bl || (rtb_Switch7_c != 0U)); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.lgciu_1_bus.discrete_word_2, + SecComputer_P.BitfromLabel5_bit_a, &rtb_Switch7_c); + rtb_NOT_bl = (rtb_Switch7_c != 0U); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.lgciu_2_bus.discrete_word_2, + SecComputer_P.BitfromLabel7_bit_m, &rtb_Switch7_c); + rtb_AND1_h = (rtb_Switch7_c != 0U); + rtb_OR3 = (rtb_NOT_bl || (rtb_Switch7_c != 0U)); + if (SecComputer_DWork.is_active_c8_SecComputer == 0U) { + SecComputer_DWork.is_active_c8_SecComputer = 1U; + SecComputer_DWork.is_c8_SecComputer = SecComputer_IN_OnGround; + rtb_OR1 = true; + } else if (SecComputer_DWork.is_c8_SecComputer == SecComputer_IN_InAir) { + if ((static_cast(rtb_OR1) > 0.1) || (static_cast(rtb_OR3) > 0.1)) { + SecComputer_DWork.is_c8_SecComputer = SecComputer_IN_OnGround; + rtb_OR1 = true; + } else { + rtb_OR1 = false; + } + } else if ((!rtb_OR1) && (!rtb_OR3)) { + SecComputer_DWork.is_c8_SecComputer = SecComputer_IN_InAir; + rtb_OR1 = false; + } else { + rtb_OR1 = true; + } + + rtb_OR3 = (SecComputer_U.in.sim_data.slew_on || SecComputer_U.in.sim_data.pause_on || + SecComputer_U.in.sim_data.tracking_mode_on_override); + if (SecComputer_DWork.is_active_c30_SecComputer == 0U) { + SecComputer_DWork.is_active_c30_SecComputer = 1U; + SecComputer_DWork.is_c30_SecComputer = SecComputer_IN_Ground; + SecComputer_B.in_flight = 0.0; + } else { + switch (SecComputer_DWork.is_c30_SecComputer) { + case SecComputer_IN_Flight: + if (rtb_OR1 && (rtb_theta < 2.5F)) { + SecComputer_DWork.on_ground_time = SecComputer_U.in.time.simulation_time; + SecComputer_DWork.is_c30_SecComputer = SecComputer_IN_FlightToGroundTransition; + } else { + SecComputer_B.in_flight = 1.0; + } + break; + + case SecComputer_IN_FlightToGroundTransition: + if (SecComputer_U.in.time.simulation_time - SecComputer_DWork.on_ground_time >= 5.0) { + SecComputer_DWork.is_c30_SecComputer = SecComputer_IN_Ground; + SecComputer_B.in_flight = 0.0; + } else if ((!rtb_OR1) || (rtb_theta >= 2.5F)) { + SecComputer_DWork.on_ground_time = 0.0; + SecComputer_DWork.is_c30_SecComputer = SecComputer_IN_Flight; + SecComputer_B.in_flight = 1.0; + } + break; + + default: + if (((!rtb_OR1) && (rtb_theta > 8.0F)) || (SecComputer_P.Constant_Value_m > 400.0)) { + SecComputer_DWork.on_ground_time = 0.0; + SecComputer_DWork.is_c30_SecComputer = SecComputer_IN_Flight; + SecComputer_B.in_flight = 1.0; + } else { + SecComputer_B.in_flight = 0.0; + } + break; + } + } + + rtb_NOT_bl = (SecComputer_B.in_flight != 0.0); + SecComputer_MATLABFunction_n(!SecComputer_U.in.discrete_inputs.yellow_low_pressure, SecComputer_U.in.time.dt, + SecComputer_P.ConfirmNode_isRisingEdge_a, SecComputer_P.ConfirmNode_timeDelay_c, &rtb_y_p, + &SecComputer_DWork.sf_MATLABFunction_ndv); + SecComputer_MATLABFunction_n(!SecComputer_U.in.discrete_inputs.blue_low_pressure, SecComputer_U.in.time.dt, + SecComputer_P.ConfirmNode1_isRisingEdge_j, SecComputer_P.ConfirmNode1_timeDelay_k, &rtb_y_k, + &SecComputer_DWork.sf_MATLABFunction_gf); + SecComputer_MATLABFunction_n(!SecComputer_U.in.discrete_inputs.green_low_pressure, SecComputer_U.in.time.dt, + SecComputer_P.ConfirmNode2_isRisingEdge, SecComputer_P.ConfirmNode2_timeDelay, &rtb_y_gp, + &SecComputer_DWork.sf_MATLABFunction_h); + rtb_BusAssignment_f_logic_ir_computation_data_n_z_g = rtb_n_z; + rtb_BusAssignment_f_logic_ir_computation_data_theta_dot_deg_s = rtb_theta_dot; + SecComputer_MATLABFunction_e(SecComputer_U.in.discrete_inputs.capt_priority_takeover_pressed, + SecComputer_P.PulseNode_isRisingEdge, &rtb_AND1_h, &SecComputer_DWork.sf_MATLABFunction_g4b); + SecComputer_MATLABFunction_e(SecComputer_U.in.discrete_inputs.fo_priority_takeover_pressed, + SecComputer_P.PulseNode1_isRisingEdge, &rtb_NOT_bl, &SecComputer_DWork.sf_MATLABFunction_nu); + if (rtb_AND1_h) { + SecComputer_DWork.pRightStickDisabled = true; + SecComputer_DWork.pLeftStickDisabled = false; + } else if (rtb_NOT_bl) { + SecComputer_DWork.pLeftStickDisabled = true; + SecComputer_DWork.pRightStickDisabled = false; + } + + if (SecComputer_DWork.pRightStickDisabled && ((!SecComputer_U.in.discrete_inputs.capt_priority_takeover_pressed) && + (!SecComputer_DWork.Delay1_DSTATE))) { + SecComputer_DWork.pRightStickDisabled = false; + } else if (SecComputer_DWork.pLeftStickDisabled) { + SecComputer_DWork.pLeftStickDisabled = (SecComputer_U.in.discrete_inputs.fo_priority_takeover_pressed || + SecComputer_DWork.Delay_DSTATE_c); + } + + SecComputer_MATLABFunction_n(SecComputer_DWork.pLeftStickDisabled && + (SecComputer_U.in.discrete_inputs.fo_priority_takeover_pressed || SecComputer_DWork.Delay_DSTATE_c), + SecComputer_U.in.time.dt, SecComputer_P.ConfirmNode1_isRisingEdge_k, SecComputer_P.ConfirmNode1_timeDelay_a, + &rtb_y_p1, &SecComputer_DWork.sf_MATLABFunction_j2); + SecComputer_MATLABFunction_n(SecComputer_DWork.pRightStickDisabled && + (SecComputer_U.in.discrete_inputs.capt_priority_takeover_pressed || SecComputer_DWork.Delay1_DSTATE), + SecComputer_U.in.time.dt, SecComputer_P.ConfirmNode_isRisingEdge_j, SecComputer_P.ConfirmNode_timeDelay_a, + &rtb_y_b, &SecComputer_DWork.sf_MATLABFunction_g24); + if (SecComputer_DWork.pLeftStickDisabled) { + rollCommand = SecComputer_P.Constant1_Value_p; + } else { + rollCommand = SecComputer_U.in.analog_inputs.capt_roll_stick_pos; + } + + if (!SecComputer_DWork.pRightStickDisabled) { + rtb_zeta_deg = SecComputer_U.in.analog_inputs.fo_roll_stick_pos; + } else { + rtb_zeta_deg = SecComputer_P.Constant1_Value_p; + } + + pair1SpdBrkCommand = rtb_zeta_deg + rollCommand; + if (pair1SpdBrkCommand > SecComputer_P.Saturation1_UpperSat) { + pair1SpdBrkCommand = SecComputer_P.Saturation1_UpperSat; + } else if (pair1SpdBrkCommand < SecComputer_P.Saturation1_LowerSat) { + pair1SpdBrkCommand = SecComputer_P.Saturation1_LowerSat; + } + + if (SecComputer_U.in.discrete_inputs.is_unit_1) { + leftElevatorAvail = ((!SecComputer_U.in.discrete_inputs.l_elev_servo_failed) && rtb_y_k); + rightElevatorAvail = ((!SecComputer_U.in.discrete_inputs.r_elev_servo_failed) && rtb_y_k); + } else { + leftElevatorAvail = ((!SecComputer_U.in.discrete_inputs.l_elev_servo_failed) && rtb_y_gp); + rightElevatorAvail = ((!SecComputer_U.in.discrete_inputs.r_elev_servo_failed) && rtb_y_p); + } + + rtb_thsAvail = ((!SecComputer_U.in.discrete_inputs.ths_motor_fault) && (rtb_y_p || rtb_y_gp)); + canEngageInPitch = ((leftElevatorAvail || rightElevatorAvail) && (!SecComputer_U.in.discrete_inputs.is_unit_3)); + if (SecComputer_U.in.discrete_inputs.is_unit_1) { + hasPriorityInPitch = (SecComputer_U.in.discrete_inputs.pitch_not_avail_elac_1 && + SecComputer_U.in.discrete_inputs.pitch_not_avail_elac_2 && + SecComputer_U.in.discrete_inputs.left_elev_not_avail_sec_opp && + SecComputer_U.in.discrete_inputs.right_elev_not_avail_sec_opp); + spoilerPair1SupplyAvail = rtb_y_k; + rtb_AND1_h = rtb_y_p; + } else { + hasPriorityInPitch = (SecComputer_U.in.discrete_inputs.is_unit_2 && + (SecComputer_U.in.discrete_inputs.pitch_not_avail_elac_1 && + SecComputer_U.in.discrete_inputs.pitch_not_avail_elac_2)); + if (SecComputer_U.in.discrete_inputs.is_unit_2) { + spoilerPair1SupplyAvail = rtb_y_gp; + rtb_AND1_h = false; + } else { + spoilerPair1SupplyAvail = rtb_y_gp; + rtb_AND1_h = rtb_y_p; + } + } + + rtb_isEngagedInPitch = (canEngageInPitch && hasPriorityInPitch); + spoilerPair1SupplyAvail = ((!SecComputer_U.in.discrete_inputs.l_spoiler_1_servo_failed) && + (!SecComputer_U.in.discrete_inputs.r_spoiler_1_servo_failed) && spoilerPair1SupplyAvail); + spoiler_pair_2_avail = ((!SecComputer_U.in.discrete_inputs.l_spoiler_2_servo_failed) && + (!SecComputer_U.in.discrete_inputs.r_spoiler_2_servo_failed) && rtb_AND1_h); + rtb_isEngagedInRoll = ((spoilerPair1SupplyAvail || spoiler_pair_2_avail) && + SecComputer_U.in.discrete_inputs.digital_output_failed_elac_1 && + SecComputer_U.in.discrete_inputs.digital_output_failed_elac_2); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.elac_1_bus.discrete_status_word_1, + SecComputer_P.BitfromLabel_bit, &rtb_Switch7_c); + SecComputer_MATLABFunction_l(&SecComputer_U.in.bus_inputs.elac_1_bus.discrete_status_word_1, &rtb_AND4_a); + rtb_y_kz = ((rtb_Switch7_c != 0U) && rtb_AND4_a); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.elac_2_bus.discrete_status_word_1, + SecComputer_P.BitfromLabel1_bit_g, &rtb_Switch7_c); + SecComputer_MATLABFunction_l(&SecComputer_U.in.bus_inputs.elac_2_bus.discrete_status_word_1, &rtb_AND1_h); + rtb_AND2_j = ((rtb_Switch7_c != 0U) && rtb_AND1_h); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.sfcc_1_bus.slat_flap_actual_position_word, + SecComputer_P.BitfromLabel2_bit_k, &rtb_Switch7_c); + SecComputer_MATLABFunction_l(&SecComputer_U.in.bus_inputs.sfcc_1_bus.slat_flap_actual_position_word, &rtb_AND4_a); + rtb_AND4_a = ((rtb_Switch7_c != 0U) && rtb_AND4_a); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.sfcc_2_bus.slat_flap_actual_position_word, + SecComputer_P.BitfromLabel3_bit_o, &rtb_Switch7_c); + rtb_NOT_bl = (rtb_Switch7_c != 0U); + SecComputer_MATLABFunction_l(&SecComputer_U.in.bus_inputs.sfcc_2_bus.slat_flap_actual_position_word, &rtb_AND1_h); + SecComputer_MATLABFunction_n(SecComputer_U.in.sim_data.slew_on, SecComputer_U.in.time.dt, + SecComputer_P.ConfirmNode_isRisingEdge_g, SecComputer_P.ConfirmNode_timeDelay_e, &rtb_NOT_bl, + &SecComputer_DWork.sf_MATLABFunction_k4); + rtb_y_l = !rtb_OR1; + abnormalCondition = ((!rtb_NOT_bl) && rtb_y_l && (((!rtb_doubleAdrFault) && ((rtb_mach > 0.91) || (rtb_alpha < + -10.0F) || (rtb_alpha > 40.0F) || (rtb_V_ias > 440.0F) || (rtb_V_ias < 60.0F))) || ((!rtb_doubleIrFault) && + ((!rtb_singleIrFault) || (!SecComputer_P.Constant_Value_l)) && ((std::abs(static_cast(rtb_phi)) > 125.0) || + ((rtb_theta > 50.0F) || (rtb_theta < -30.0F)))))); + SecComputer_DWork.abnormalConditionWasActive = (abnormalCondition || (rtb_y_l && + SecComputer_DWork.abnormalConditionWasActive)); + if (rtb_doubleIrFault || ((SecComputer_B.in_flight != 0.0) && ((rtb_OR && (!rtb_OR6)) || ((rtb_AND4_a || + ((rtb_Switch7_c != 0U) && rtb_AND1_h)) && rtb_OR6)))) { + rtb_pitchLawCapability = pitch_efcs_law::DirectLaw; + } else if ((rtb_singleAdrFault && SecComputer_P.Constant2_Value_c) || rtb_doubleAdrFault || + SecComputer_DWork.abnormalConditionWasActive || ((!rtb_y_kz) && (!rtb_AND2_j) && ((!leftElevatorAvail) || + (!rightElevatorAvail)))) { + rtb_pitchLawCapability = pitch_efcs_law::AlternateLaw2; + } else { + rtb_pitchLawCapability = pitch_efcs_law::AlternateLaw1; + } + + if (rtb_isEngagedInPitch) { + rtb_activePitchLaw = rtb_pitchLawCapability; + } else { + rtb_activePitchLaw = pitch_efcs_law::None; + } + + if (!SecComputer_DWork.pRightStickDisabled) { + rtb_zeta_deg = SecComputer_U.in.analog_inputs.fo_pitch_stick_pos; + } else { + rtb_zeta_deg = SecComputer_P.Constant_Value_p; + } + + if (SecComputer_DWork.pLeftStickDisabled) { + u0_0 = SecComputer_P.Constant_Value_p; + } else { + u0_0 = SecComputer_U.in.analog_inputs.capt_pitch_stick_pos; + } + + u0_0 += rtb_zeta_deg; + if (u0_0 > SecComputer_P.Saturation_UpperSat_d) { + u0_0 = SecComputer_P.Saturation_UpperSat_d; + } else if (u0_0 < SecComputer_P.Saturation_LowerSat_h) { + u0_0 = SecComputer_P.Saturation_LowerSat_h; + } + + SecComputer_MATLABFunction_e(SecComputer_B.in_flight != 0.0, SecComputer_P.PulseNode_isRisingEdge_h, &rtb_y_kz, + &SecComputer_DWork.sf_MATLABFunction_b4); + rtb_y_l = (SecComputer_U.in.discrete_inputs.pitch_not_avail_elac_1 && + SecComputer_U.in.discrete_inputs.pitch_not_avail_elac_2); + rtb_AND1_h = (SecComputer_U.in.discrete_inputs.is_unit_1 && rtb_thsAvail && rtb_y_l); + SecComputer_DWork.Memory_PreviousInput = SecComputer_P.Logic_table[((((!rtb_AND1_h) || (std::abs + (SecComputer_U.in.analog_inputs.ths_pos_deg) <= SecComputer_P.CompareToConstant1_const) || + SecComputer_U.in.discrete_inputs.ths_override_active) + (static_cast(rtb_y_kz) << 1)) << 1) + + SecComputer_DWork.Memory_PreviousInput]; + rtb_NOT_bl = (rtb_AND1_h && SecComputer_DWork.Memory_PreviousInput); + rtb_AND4_a = !abnormalCondition; + rtb_y_kz = ((rtb_isEngagedInPitch && (SecComputer_B.in_flight != 0.0) && ((rtb_activePitchLaw != + SecComputer_P.EnumeratedConstant_Value_f) && rtb_AND4_a)) || rtb_NOT_bl); + rtb_AND2_j = rtb_NOT_bl; + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.elac_1_bus.discrete_status_word_2, + SecComputer_P.BitfromLabel7_bit_g, &rtb_Switch7_c); + SecComputer_MATLABFunction_l(&SecComputer_U.in.bus_inputs.elac_1_bus.discrete_status_word_2, &rtb_AND1_h); + rtb_NOT_bl = ((rtb_Switch7_c != 0U) && rtb_AND1_h); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.elac_2_bus.discrete_status_word_2, + SecComputer_P.BitfromLabel6_bit_f, &rtb_Switch7_c); + SecComputer_MATLABFunction_l(&SecComputer_U.in.bus_inputs.elac_2_bus.discrete_status_word_2, &rtb_AND4_a); + rtb_AND2_p = ((rtb_Switch7_c != 0U) && rtb_AND4_a); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.sfcc_1_bus.slat_flap_actual_position_word, + SecComputer_P.BitfromLabel_bit_l, &rtb_Switch7_c); + SecComputer_MATLABFunction_l(&SecComputer_U.in.bus_inputs.sfcc_1_bus.slat_flap_actual_position_word, &rtb_AND4_a); + rtb_AND4_a = ((rtb_Switch7_c != 0U) && rtb_AND4_a); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.sfcc_2_bus.slat_flap_actual_position_word, + SecComputer_P.BitfromLabel1_bit_p, &rtb_Switch7_c); + SecComputer_MATLABFunction_l(&SecComputer_U.in.bus_inputs.sfcc_2_bus.slat_flap_actual_position_word, &rtb_AND1_h); + rtb_AND1_h = ((rtb_Switch7_c != 0U) && rtb_AND1_h); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.elac_1_bus.discrete_status_word_1, + SecComputer_P.BitfromLabel5_bit_p, &rtb_Switch7_c); + rtb_NOT2_b = (rtb_Switch7_c == 0U); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.elac_1_bus.discrete_status_word_1, + SecComputer_P.BitfromLabel4_bit_e, &rtb_Switch7_c); + rtb_NOT2_b = (rtb_NOT2_b || (rtb_Switch7_c == 0U)); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.elac_2_bus.discrete_status_word_1, + SecComputer_P.BitfromLabel3_bit_oz, &rtb_Switch7_c); + rtb_NOT_g = (rtb_Switch7_c == 0U); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.elac_2_bus.discrete_status_word_1, + SecComputer_P.BitfromLabel2_bit_p, &rtb_Switch7_c); + rtb_NOT_bl = (rtb_NOT_bl || rtb_AND2_p || (rtb_AND4_a || rtb_AND1_h) || (((!rtb_y_l) && rtb_NOT2_b && (rtb_NOT_g || + (rtb_Switch7_c == 0U))) || (rtb_y_l && ((SecComputer_U.in.discrete_inputs.left_elev_not_avail_sec_opp && + (!leftElevatorAvail)) || (SecComputer_U.in.discrete_inputs.right_elev_not_avail_sec_opp && (!rightElevatorAvail))))) + || ((SecComputer_U.in.analog_inputs.thr_lever_1_pos >= SecComputer_P.CompareToConstant3_const) || + (SecComputer_U.in.analog_inputs.thr_lever_2_pos >= SecComputer_P.CompareToConstant4_const))); + SecComputer_MATLABFunction_n(SecComputer_U.in.analog_inputs.spd_brk_lever_pos < + SecComputer_P.CompareToConstant_const, SecComputer_U.in.time.dt, SecComputer_P.ConfirmNode_isRisingEdge_e, + SecComputer_P.ConfirmNode_timeDelay_eq, &rtb_y_l, &SecComputer_DWork.sf_MATLABFunction_fh); + SecComputer_DWork.Memory_PreviousInput_f = SecComputer_P.Logic_table_i[(((static_cast(rtb_NOT_bl) << 1) + + rtb_y_l) << 1) + SecComputer_DWork.Memory_PreviousInput_f]; + rtb_y_l = (rtb_NOT_bl || SecComputer_DWork.Memory_PreviousInput_f); + rtb_AND2_p = rtb_y_p; + rtb_NOT2_b = rtb_y_k; + rtb_NOT_g = rtb_y_gp; + SecComputer_MATLABFunction_e(rtb_OR1, SecComputer_P.PulseNode3_isRisingEdge, &rtb_y_gp, + &SecComputer_DWork.sf_MATLABFunction_nd); + rtb_NOT_bl = (rtb_y_gp || ((SecComputer_U.in.analog_inputs.wheel_speed_left < + SecComputer_P.CompareToConstant11_const) && (SecComputer_U.in.analog_inputs.wheel_speed_right < + SecComputer_P.CompareToConstant12_const))); + SecComputer_MATLABFunction_e(rtb_OR1, SecComputer_P.PulseNode2_isRisingEdge, &rtb_y_gp, + &SecComputer_DWork.sf_MATLABFunction_n); + SecComputer_DWork.Memory_PreviousInput_n = SecComputer_P.Logic_table_ii[(((static_cast(rtb_NOT_bl) << 1) + + rtb_y_gp) << 1) + SecComputer_DWork.Memory_PreviousInput_n]; + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.lgciu_1_bus.discrete_word_2, + SecComputer_P.BitfromLabel4_bit_a, &rtb_Switch7_c); + rtb_AND1_h = (rtb_Switch7_c != 0U); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.lgciu_2_bus.discrete_word_2, + SecComputer_P.BitfromLabel6_bit_d, &rtb_Switch7_c); + rtb_y_gp = (rtb_AND1_h && (rtb_Switch7_c != 0U)); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.lgciu_1_bus.discrete_word_2, + SecComputer_P.BitfromLabel5_bit_i, &rtb_Switch7_c); + rtb_AND1_h = (rtb_Switch7_c != 0U); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.lgciu_2_bus.discrete_word_2, + SecComputer_P.BitfromLabel7_bit_ms, &rtb_Switch7_c); + SecComputer_MATLABFunction_e(rtb_y_gp || (rtb_AND1_h && (rtb_Switch7_c != 0U)), + SecComputer_P.PulseNode1_isRisingEdge_k, &rtb_y_k, &SecComputer_DWork.sf_MATLABFunction_a); + rtb_NOT_bl = (SecComputer_U.in.analog_inputs.spd_brk_lever_pos < SecComputer_P.CompareToConstant_const_m); + rtb_AND1_h = ((((SecComputer_U.in.analog_inputs.spd_brk_lever_pos > SecComputer_P.CompareToConstant15_const) || + rtb_NOT_bl) && ((SecComputer_U.in.analog_inputs.thr_lever_1_pos <= + SecComputer_P.CompareToConstant1_const_l) && (SecComputer_U.in.analog_inputs.thr_lever_2_pos <= + SecComputer_P.CompareToConstant2_const))) || (((SecComputer_U.in.analog_inputs.thr_lever_1_pos < + SecComputer_P.CompareToConstant3_const_a) && (SecComputer_U.in.analog_inputs.thr_lever_2_pos <= + SecComputer_P.CompareToConstant4_const_j)) || ((SecComputer_U.in.analog_inputs.thr_lever_1_pos <= + SecComputer_P.CompareToConstant13_const) && (SecComputer_U.in.analog_inputs.thr_lever_2_pos < + SecComputer_P.CompareToConstant14_const)))); + SecComputer_DWork.Delay1_DSTATE_i = (rtb_AND1_h && (rtb_y_k || ((SecComputer_U.in.analog_inputs.wheel_speed_left >= + SecComputer_P.CompareToConstant5_const) && (SecComputer_U.in.analog_inputs.wheel_speed_right >= + SecComputer_P.CompareToConstant6_const) && SecComputer_DWork.Memory_PreviousInput_n) || + SecComputer_DWork.Delay1_DSTATE_i)); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.lgciu_1_bus.discrete_word_2, + SecComputer_P.BitfromLabel_bit_g, &rtb_Switch7_c); + rtb_AND4_a = (rtb_Switch7_c != 0U); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.lgciu_2_bus.discrete_word_2, + SecComputer_P.BitfromLabel2_bit_l, &rtb_Switch7_c); + rtb_y_gp = (rtb_AND4_a || (rtb_Switch7_c != 0U)); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.lgciu_1_bus.discrete_word_2, + SecComputer_P.BitfromLabel1_bit_a, &rtb_Switch7_c); + rtb_AND4_a = (rtb_Switch7_c != 0U); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.lgciu_2_bus.discrete_word_2, + SecComputer_P.BitfromLabel3_bit_m, &rtb_Switch7_c); + SecComputer_Y.out.discrete_outputs.batt_power_supply = (rtb_Switch7_c != 0U); + SecComputer_MATLABFunction_e(rtb_y_gp || (rtb_AND4_a || (rtb_Switch7_c != 0U)), + SecComputer_P.PulseNode_isRisingEdge_hj, &rtb_y_p, &SecComputer_DWork.sf_MATLABFunction_e3); + SecComputer_DWork.Delay_DSTATE_n = (rtb_AND1_h && (rtb_y_p || SecComputer_DWork.Delay_DSTATE_n)); + rtb_AND4_a = (SecComputer_U.in.analog_inputs.thr_lever_2_pos <= SecComputer_P.CompareToConstant8_const); + SecComputer_DWork.Delay2_DSTATE = (rtb_NOT_bl && ((SecComputer_U.in.analog_inputs.thr_lever_1_pos <= + SecComputer_P.CompareToConstant7_const) && rtb_AND4_a) && (rtb_y_k || SecComputer_DWork.Delay2_DSTATE)); + rtb_AND1_h = ((!SecComputer_DWork.Delay1_DSTATE_i) && (SecComputer_DWork.Delay_DSTATE_n || + SecComputer_DWork.Delay2_DSTATE)); + SecComputer_Y.out.logic.total_sidestick_roll_command = pair1SpdBrkCommand; + rtb_y_k = rtb_NOT_bl; + if (SecComputer_DWork.Delay1_DSTATE_i) { + rtb_zeta_deg = SecComputer_P.Constant_Value; + } else if (rtb_AND1_h) { + rtb_zeta_deg = SecComputer_P.Constant1_Value; + } else { + rtb_zeta_deg = SecComputer_P.Constant2_Value; + } + + SecComputer_RateLimiter(rtb_zeta_deg, SecComputer_P.RateLimiterVariableTs6_up, + SecComputer_P.RateLimiterVariableTs6_lo, SecComputer_U.in.time.dt, + SecComputer_P.RateLimiterVariableTs6_InitialCondition, &rtb_handleIndex, &SecComputer_DWork.sf_RateLimiter_c); + rtb_NOT_bl = (SecComputer_DWork.Delay1_DSTATE_i || rtb_AND1_h); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.elac_1_bus.discrete_status_word_2, + SecComputer_P.BitfromLabel4_bit_m, &rtb_Switch7_c); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.elac_2_bus.discrete_status_word_2, + SecComputer_P.BitfromLabel5_bit_h, &rtb_y); + if (rtb_y_l) { + rtb_Switch3 = SecComputer_P.Constant3_Value; + } else { + if ((rtb_Switch7_c != 0U) || (rtb_y != 0U)) { + rtb_Switch5 = SecComputer_P.Constant4_Value_k; + } else { + rtb_Switch5 = SecComputer_P.Constant5_Value; + } + + if (SecComputer_U.in.analog_inputs.spd_brk_lever_pos <= rtb_Switch5) { + rtb_Switch5 *= SecComputer_P.Gain_Gain; + if (SecComputer_U.in.analog_inputs.spd_brk_lever_pos >= rtb_Switch5) { + rtb_Switch5 = SecComputer_U.in.analog_inputs.spd_brk_lever_pos; + } + } + + rtb_Switch3 = look1_binlxpw(rtb_Switch5, SecComputer_P.uDLookupTable_bp01Data, + SecComputer_P.uDLookupTable_tableData, 4U); + } + + SecComputer_RateLimiter(rtb_Switch3, SecComputer_P.RateLimiterVariableTs1_up, + SecComputer_P.RateLimiterVariableTs1_lo, SecComputer_U.in.time.dt, + SecComputer_P.RateLimiterVariableTs1_InitialCondition, &rtb_Switch5, &SecComputer_DWork.sf_RateLimiter); + LawMDLOBJ1.step(&SecComputer_U.in.time.dt, &pair1SpdBrkCommand, &rtb_Switch3, &rtb_zeta_deg); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.elac_1_bus.discrete_status_word_1, + SecComputer_P.BitfromLabel_bit_a, &rtb_Switch7_c); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.elac_2_bus.discrete_status_word_1, + SecComputer_P.BitfromLabel1_bit_c, &rtb_y); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.elac_1_bus.discrete_status_word_1, + SecComputer_P.BitfromLabel2_bit_o, &rtb_y_m); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.elac_2_bus.discrete_status_word_1, + SecComputer_P.BitfromLabel3_bit_j, &rtb_y_d); + if (SecComputer_U.in.bus_inputs.elac_1_bus.roll_spoiler_command_deg.SSM == static_cast(SignStatusMatrix:: + NormalOperation)) { + pair1SpdBrkCommand = SecComputer_U.in.bus_inputs.elac_1_bus.roll_spoiler_command_deg.Data; + rtb_y_gp = (rtb_Switch7_c != 0U); + rtb_y_p = (rtb_y_m != 0U); + } else if (SecComputer_U.in.bus_inputs.elac_2_bus.roll_spoiler_command_deg.SSM == static_cast + (SignStatusMatrix::NormalOperation)) { + pair1SpdBrkCommand = SecComputer_U.in.bus_inputs.elac_2_bus.roll_spoiler_command_deg.Data; + rtb_y_gp = (rtb_y != 0U); + rtb_y_p = (rtb_y_d != 0U); + } else { + pair1SpdBrkCommand = rtb_Switch3 * 35.0 / 25.0; + rtb_y_gp = true; + rtb_y_p = true; + } + + rollCommand = std::fmax(std::fmin(pair1SpdBrkCommand, 35.0), -35.0); + if (SecComputer_U.in.discrete_inputs.is_unit_1) { + if (rtb_y_gp) { + pair1RollCommand = rollCommand; + } else { + pair1RollCommand = 0.0; + } + + rtb_Switch3 = rollCommand; + pair1SpdBrkCommand = rtb_Switch5; + rtb_zeta_deg = rtb_Switch5; + } else if (SecComputer_U.in.discrete_inputs.is_unit_2) { + pair1RollCommand = rollCommand; + rtb_Switch3 = 0.0; + pair1SpdBrkCommand = 0.0; + rtb_zeta_deg = 0.0; + } else { + pair1RollCommand = 0.0; + if (rtb_y_p) { + rtb_Switch3 = rollCommand; + } else { + rtb_Switch3 = 0.0; + } + + pair1SpdBrkCommand = 0.0; + rtb_zeta_deg = rtb_Switch5 / 2.0; + } + + if (rollCommand >= 0.0) { + rollCommand = pair1SpdBrkCommand - pair1RollCommand; + pair1RollCommand = rtb_zeta_deg - rtb_Switch3; + } else { + rollCommand = pair1SpdBrkCommand; + pair1SpdBrkCommand += pair1RollCommand; + pair1RollCommand = rtb_zeta_deg; + rtb_zeta_deg += rtb_Switch3; + } + + if (rtb_NOT_bl) { + u0 = rtb_handleIndex; + } else { + u0 = std::fmax(rollCommand - (pair1SpdBrkCommand - std::fmax(pair1SpdBrkCommand, -50.0)), -50.0); + } + + if (u0 > SecComputer_P.Saturation_UpperSat_n) { + u0 = SecComputer_P.Saturation_UpperSat_n; + } else if (u0 < SecComputer_P.Saturation_LowerSat_n) { + u0 = SecComputer_P.Saturation_LowerSat_n; + } + + SecComputer_RateLimiter_b(u0, SecComputer_P.RateLimiterGenericVariableTs_up, + SecComputer_P.RateLimiterGenericVariableTs_lo, SecComputer_U.in.time.dt, + SecComputer_U.in.analog_inputs.left_spoiler_1_pos_deg, !spoilerPair1SupplyAvail, &rtb_Switch3, + &SecComputer_DWork.sf_RateLimiter_b); + if (rtb_NOT_bl) { + u0 = rtb_handleIndex; + } else { + u0 = std::fmax(pair1SpdBrkCommand - (rollCommand - std::fmax(rollCommand, -50.0)), -50.0); + } + + if (u0 > SecComputer_P.Saturation1_UpperSat_e) { + u0 = SecComputer_P.Saturation1_UpperSat_e; + } else if (u0 < SecComputer_P.Saturation1_LowerSat_f) { + u0 = SecComputer_P.Saturation1_LowerSat_f; + } + + SecComputer_RateLimiter_b(u0, SecComputer_P.RateLimiterGenericVariableTs1_up, + SecComputer_P.RateLimiterGenericVariableTs1_lo, SecComputer_U.in.time.dt, + SecComputer_U.in.analog_inputs.right_spoiler_1_pos_deg, !spoilerPair1SupplyAvail, &rollCommand, + &SecComputer_DWork.sf_RateLimiter_a); + if (rtb_NOT_bl) { + u0 = rtb_handleIndex; + } else { + u0 = std::fmax(pair1RollCommand - (rtb_zeta_deg - std::fmax(rtb_zeta_deg, -50.0)), -50.0); + } + + if (u0 > SecComputer_P.Saturation2_UpperSat) { + u0 = SecComputer_P.Saturation2_UpperSat; + } else if (u0 < SecComputer_P.Saturation2_LowerSat) { + u0 = SecComputer_P.Saturation2_LowerSat; + } + + SecComputer_RateLimiter_b(u0, SecComputer_P.RateLimiterGenericVariableTs2_up, + SecComputer_P.RateLimiterGenericVariableTs2_lo, SecComputer_U.in.time.dt, + SecComputer_U.in.analog_inputs.left_spoiler_2_pos_deg, !spoiler_pair_2_avail, &pair1SpdBrkCommand, + &SecComputer_DWork.sf_RateLimiter_k); + if (!rtb_NOT_bl) { + rtb_handleIndex = std::fmax(rtb_zeta_deg - (pair1RollCommand - std::fmax(pair1RollCommand, -50.0)), -50.0); + } + + if (rtb_handleIndex > SecComputer_P.Saturation3_UpperSat) { + rtb_zeta_deg = SecComputer_P.Saturation3_UpperSat; + } else if (rtb_handleIndex < SecComputer_P.Saturation3_LowerSat) { + rtb_zeta_deg = SecComputer_P.Saturation3_LowerSat; + } else { + rtb_zeta_deg = rtb_handleIndex; + } + + SecComputer_RateLimiter_b(rtb_zeta_deg, SecComputer_P.RateLimiterGenericVariableTs3_up, + SecComputer_P.RateLimiterGenericVariableTs3_lo, SecComputer_U.in.time.dt, + SecComputer_U.in.analog_inputs.right_spoiler_2_pos_deg, !spoiler_pair_2_avail, &rtb_handleIndex, + &SecComputer_DWork.sf_RateLimiter_b4); + rtb_zeta_deg = rollCommand; + pair1RollCommand = pair1SpdBrkCommand; + u0 = rtb_handleIndex; + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.sfcc_1_bus.slat_flap_system_status_word, + SecComputer_P.BitfromLabel_bit_a1, &rtb_y_d); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.sfcc_1_bus.slat_flap_system_status_word, + SecComputer_P.BitfromLabel1_bit_gf, &rtb_y_m); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.sfcc_1_bus.slat_flap_system_status_word, + SecComputer_P.BitfromLabel2_bit_n, &rtb_y); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.sfcc_1_bus.slat_flap_system_status_word, + SecComputer_P.BitfromLabel3_bit_l, &rtb_Switch9_c); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.sfcc_1_bus.slat_flap_system_status_word, + SecComputer_P.BitfromLabel4_bit_n, &rtb_DataTypeConversion1); + SecComputer_MATLABFunction(&SecComputer_U.in.bus_inputs.sfcc_1_bus.slat_flap_system_status_word, + SecComputer_P.BitfromLabel5_bit_m, &rtb_Switch7_c); + if (rtb_y_d != 0U) { + rtb_handleIndex = 0.0; + } else if ((rtb_y_m != 0U) && (rtb_Switch7_c != 0U)) { + rtb_handleIndex = 1.0; + } else if ((rtb_y_m != 0U) && (rtb_Switch7_c == 0U)) { + rtb_handleIndex = 2.0; + } else if (rtb_y != 0U) { + rtb_handleIndex = 3.0; + } else if (rtb_Switch9_c != 0U) { + rtb_handleIndex = 4.0; + } else if (rtb_DataTypeConversion1 != 0U) { + rtb_handleIndex = 5.0; + } else { + rtb_handleIndex = 0.0; + } + + pair1SpdBrkCommand = (SecComputer_B.in_flight != 0.0); + rtb_y_gp = (rtb_OR3 || ((static_cast(rtb_activePitchLaw) != SecComputer_P.CompareToConstant2_const_f) && ( + static_cast(rtb_activePitchLaw) != SecComputer_P.CompareToConstant3_const_o))); + rtb_y_p = (rtb_activePitchLaw != SecComputer_P.EnumeratedConstant_Value_i); + LawMDLOBJ2.step(&SecComputer_U.in.time.dt, &rtb_BusAssignment_f_logic_ir_computation_data_n_z_g, + &rtb_Switch5_tmp_tmp, &rtb_Switch6_m, &rtb_BusAssignment_f_logic_ir_computation_data_theta_dot_deg_s, + (const_cast(&SecComputer_RGND)), &SecComputer_U.in.analog_inputs.ths_pos_deg, + &rtb_eta_trim_limit_lo_d, &rtb_BusConversion_InsertedFor_BusAssignment_at_inport_8_BusCreator1_mach, + &rtb_BusConversion_InsertedFor_BusAssignment_at_inport_8_BusCreator1_V_tas_kn, &rtb_handleIndex, ( + const_cast(&SecComputer_RGND)), (const_cast(&SecComputer_RGND)), &u0_0, &pair1SpdBrkCommand, + &rtb_y_gp, &rtb_y_p, &rtb_eta_deg, &rtb_eta_trim_dot_deg_s, &rtb_eta_trim_limit_lo, + &rtb_eta_trim_limit_up); + LawMDLOBJ3.step(&SecComputer_U.in.time.dt, &u0_0, &rtb_handleIndex, &rollCommand, &rtb_eta_trim_limit_lo_d, + &pair1SpdBrkCommand); + switch (static_cast(rtb_activePitchLaw)) { + case 1: + case 2: + rtb_handleIndex = rtb_eta_deg; + break; + + case 3: + break; + + default: + rtb_handleIndex = SecComputer_P.Constant1_Value_px; + break; + } + + switch (static_cast(rtb_activePitchLaw)) { + case 1: + case 2: + pair1SpdBrkCommand = rtb_eta_trim_limit_up; + break; + + case 3: + break; + + default: + pair1SpdBrkCommand = SecComputer_P.Constant2_Value_g; + break; + } + + if (rtb_AND2_j) { + rollCommand = SecComputer_P.Gain_Gain_m * SecComputer_DWork.Delay_DSTATE; + if (rollCommand > SecComputer_P.Saturation_UpperSat) { + rollCommand = SecComputer_P.Saturation_UpperSat; + } else if (rollCommand < SecComputer_P.Saturation_LowerSat) { + rollCommand = SecComputer_P.Saturation_LowerSat; + } + } else if (SecComputer_U.in.discrete_inputs.ths_override_active) { + rollCommand = SecComputer_P.Constant_Value_a; + } else { + switch (static_cast(rtb_activePitchLaw)) { + case 1: + case 2: + rollCommand = rtb_eta_trim_dot_deg_s; + break; + + case 3: + break; + + default: + rollCommand = SecComputer_P.Constant1_Value_px; + break; + } + } + + rollCommand = SecComputer_P.DiscreteTimeIntegratorVariableTsLimit_Gain * rollCommand * SecComputer_U.in.time.dt; + SecComputer_DWork.icLoad = ((!rtb_y_kz) || SecComputer_DWork.icLoad); + if (SecComputer_DWork.icLoad) { + SecComputer_DWork.Delay_DSTATE_l = SecComputer_U.in.analog_inputs.ths_pos_deg - rollCommand; + } + + SecComputer_DWork.Delay_DSTATE = rollCommand + SecComputer_DWork.Delay_DSTATE_l; + if (SecComputer_DWork.Delay_DSTATE > pair1SpdBrkCommand) { + SecComputer_DWork.Delay_DSTATE = pair1SpdBrkCommand; + } else { + switch (static_cast(rtb_activePitchLaw)) { + case 1: + case 2: + rtb_eta_trim_limit_lo_d = rtb_eta_trim_limit_lo; + break; + + case 3: + break; + + default: + rtb_eta_trim_limit_lo_d = SecComputer_P.Constant3_Value_i; + break; + } + + if (SecComputer_DWork.Delay_DSTATE < rtb_eta_trim_limit_lo_d) { + SecComputer_DWork.Delay_DSTATE = rtb_eta_trim_limit_lo_d; + } + } + + SecComputer_Y.out.laws.pitch_law_outputs.elevator_command_deg = rtb_handleIndex; + if (SecComputer_U.in.discrete_inputs.is_unit_1) { + rtb_Switch8 = SecComputer_U.in.bus_inputs.elac_2_bus.elevator_double_pressurization_command_deg; + } else if (SecComputer_U.in.discrete_inputs.is_unit_2) { + rtb_Switch8 = SecComputer_U.in.bus_inputs.elac_1_bus.elevator_double_pressurization_command_deg; + } else { + real32_T tmp; + tmp = std::fmod(std::floor(SecComputer_P.Constant1_Value_m), 4.2949673E+9F); + rtb_Switch8.SSM = tmp < 0.0F ? static_cast(-static_cast(static_cast(-tmp))) : + static_cast(tmp); + rtb_Switch8.Data = SecComputer_P.Constant1_Value_m; + } + + SecComputer_MATLABFunction_l(&rtb_Switch8, &rtb_AND4_a); + if (SecComputer_U.in.discrete_inputs.is_unit_1) { + rtb_y_gp = SecComputer_U.in.discrete_inputs.pitch_not_avail_elac_1; + } else { + rtb_y_gp = SecComputer_U.in.discrete_inputs.pitch_not_avail_elac_2; + } + + rtb_NOT_bl = (rtb_y_gp && (!rtb_isEngagedInPitch) && rtb_AND4_a); + if (rtb_NOT_bl) { + rtb_handleIndex = rtb_Switch8.Data; + } + + SecComputer_MATLABFunction_n(rtb_AND2_p || rtb_NOT2_b || rtb_NOT_g, SecComputer_U.in.time.dt, + SecComputer_P.ConfirmNode_isRisingEdge_c, SecComputer_P.ConfirmNode_timeDelay_m, + &SecComputer_Y.out.discrete_outputs.batt_power_supply, &SecComputer_DWork.sf_MATLABFunction_i); + SecComputer_Y.out.bus_outputs.discrete_status_word_2.Data = static_cast + (SecComputer_U.in.analog_inputs.thr_lever_2_pos); + rtb_VectorConcatenate[0] = (SecComputer_U.in.discrete_inputs.l_spoiler_1_servo_failed || + SecComputer_U.in.discrete_inputs.r_spoiler_1_servo_failed); + rtb_VectorConcatenate[1] = (SecComputer_U.in.discrete_inputs.l_spoiler_2_servo_failed || + SecComputer_U.in.discrete_inputs.r_spoiler_2_servo_failed); + rtb_VectorConcatenate[2] = SecComputer_U.in.discrete_inputs.l_elev_servo_failed; + rtb_VectorConcatenate[3] = SecComputer_U.in.discrete_inputs.r_elev_servo_failed; + rtb_VectorConcatenate[4] = spoilerPair1SupplyAvail; + rtb_VectorConcatenate[5] = spoiler_pair_2_avail; + rtb_VectorConcatenate[6] = leftElevatorAvail; + rtb_VectorConcatenate[7] = rightElevatorAvail; + rtb_VectorConcatenate[8] = (rtb_activePitchLaw == pitch_efcs_law::AlternateLaw2); + rtb_VectorConcatenate[9] = ((rtb_activePitchLaw == pitch_efcs_law::AlternateLaw1) || (rtb_activePitchLaw == + pitch_efcs_law::AlternateLaw2)); + rtb_VectorConcatenate[10] = (rtb_activePitchLaw == pitch_efcs_law::DirectLaw); + rtb_VectorConcatenate[11] = rtb_isEngagedInRoll; + rtb_VectorConcatenate[12] = rtb_isEngagedInPitch; + rtb_VectorConcatenate[13] = SecComputer_P.Constant8_Value; + rtb_VectorConcatenate[14] = SecComputer_DWork.Delay1_DSTATE_i; + rtb_VectorConcatenate[15] = rtb_y_k; + rtb_VectorConcatenate[16] = SecComputer_P.Constant8_Value; + rtb_VectorConcatenate[17] = SecComputer_P.Constant8_Value; + rtb_VectorConcatenate[18] = SecComputer_P.Constant8_Value; + SecComputer_MATLABFunction_c(rtb_VectorConcatenate, &SecComputer_Y.out.bus_outputs.discrete_status_word_1.Data); + rtb_VectorConcatenate[0] = SecComputer_P.Constant7_Value; + rtb_VectorConcatenate[1] = SecComputer_P.Constant7_Value; + rtb_VectorConcatenate[2] = SecComputer_DWork.pLeftStickDisabled; + rtb_VectorConcatenate[3] = SecComputer_DWork.pRightStickDisabled; + rtb_VectorConcatenate[4] = rtb_y_p1; + rtb_VectorConcatenate[5] = rtb_y_b; + rtb_VectorConcatenate[6] = rtb_OR6; + rtb_VectorConcatenate[7] = rtb_OR; + rtb_VectorConcatenate[8] = SecComputer_P.Constant10_Value; + rtb_VectorConcatenate[9] = SecComputer_P.Constant10_Value; + rtb_VectorConcatenate[10] = SecComputer_P.Constant10_Value; + rtb_VectorConcatenate[11] = SecComputer_P.Constant10_Value; + rtb_VectorConcatenate[12] = SecComputer_P.Constant10_Value; + rtb_VectorConcatenate[13] = SecComputer_P.Constant10_Value; + rtb_VectorConcatenate[14] = SecComputer_P.Constant10_Value; + rtb_VectorConcatenate[15] = SecComputer_P.Constant10_Value; + rtb_VectorConcatenate[16] = SecComputer_P.Constant10_Value; + rtb_VectorConcatenate[17] = SecComputer_P.Constant10_Value; + rtb_VectorConcatenate[18] = SecComputer_P.Constant10_Value; + SecComputer_MATLABFunction_c(rtb_VectorConcatenate, &SecComputer_Y.out.bus_outputs.discrete_status_word_2.Data); + SecComputer_Y.out.data = SecComputer_U.in; + SecComputer_Y.out.laws.lateral_law_outputs.left_spoiler_1_command_deg = rtb_Switch3; + SecComputer_Y.out.laws.lateral_law_outputs.right_spoiler_1_command_deg = rtb_zeta_deg; + SecComputer_Y.out.laws.lateral_law_outputs.left_spoiler_2_command_deg = pair1RollCommand; + SecComputer_Y.out.laws.lateral_law_outputs.right_spoiler_2_command_deg = u0; + SecComputer_Y.out.laws.lateral_law_outputs.speedbrake_command_deg = rtb_Switch5; + SecComputer_Y.out.laws.pitch_law_outputs.ths_command_deg = SecComputer_DWork.Delay_DSTATE; + SecComputer_Y.out.logic.on_ground = rtb_OR1; + SecComputer_Y.out.logic.pitch_law_in_flight = (SecComputer_B.in_flight != 0.0); + SecComputer_Y.out.logic.tracking_mode_on = rtb_OR3; + SecComputer_Y.out.logic.pitch_law_capability = rtb_pitchLawCapability; + SecComputer_Y.out.logic.active_pitch_law = rtb_activePitchLaw; + SecComputer_Y.out.logic.abnormal_condition_law_active = abnormalCondition; + SecComputer_Y.out.logic.is_engaged_in_pitch = rtb_isEngagedInPitch; + SecComputer_Y.out.logic.can_engage_in_pitch = canEngageInPitch; + SecComputer_Y.out.logic.has_priority_in_pitch = hasPriorityInPitch; + SecComputer_Y.out.logic.left_elevator_avail = leftElevatorAvail; + SecComputer_Y.out.logic.right_elevator_avail = rightElevatorAvail; + SecComputer_Y.out.logic.ths_avail = rtb_thsAvail; + SecComputer_Y.out.logic.ths_active_commanded = rtb_y_kz; + SecComputer_Y.out.logic.ths_ground_setting_active = rtb_AND2_j; + SecComputer_Y.out.logic.is_engaged_in_roll = rtb_isEngagedInRoll; + SecComputer_Y.out.logic.spoiler_pair_1_avail = spoilerPair1SupplyAvail; + SecComputer_Y.out.logic.spoiler_pair_2_avail = spoiler_pair_2_avail; + SecComputer_Y.out.logic.is_yellow_hydraulic_power_avail = rtb_AND2_p; + SecComputer_Y.out.logic.is_blue_hydraulic_power_avail = rtb_NOT2_b; + SecComputer_Y.out.logic.is_green_hydraulic_power_avail = rtb_NOT_g; + SecComputer_Y.out.logic.left_sidestick_disabled = SecComputer_DWork.pLeftStickDisabled; + SecComputer_Y.out.logic.right_sidestick_disabled = SecComputer_DWork.pRightStickDisabled; + SecComputer_Y.out.logic.left_sidestick_priority_locked = rtb_y_p1; + SecComputer_Y.out.logic.right_sidestick_priority_locked = rtb_y_b; + SecComputer_Y.out.logic.total_sidestick_pitch_command = u0_0; + SecComputer_Y.out.logic.ground_spoilers_armed = rtb_y_k; + SecComputer_Y.out.logic.ground_spoilers_out = SecComputer_DWork.Delay1_DSTATE_i; + SecComputer_Y.out.logic.partial_lift_dumping_active = rtb_AND1_h; + SecComputer_Y.out.logic.speed_brake_inhibited = rtb_y_l; + SecComputer_Y.out.logic.single_adr_failure = rtb_singleAdrFault; + SecComputer_Y.out.logic.double_adr_failure = rtb_doubleAdrFault; + SecComputer_Y.out.logic.cas_or_mach_disagree = SecComputer_P.Constant2_Value_c; + SecComputer_Y.out.logic.alpha_disagree = SecComputer_P.Constant2_Value_c; + SecComputer_Y.out.logic.single_ir_failure = rtb_singleIrFault; + SecComputer_Y.out.logic.double_ir_failure = rtb_doubleIrFault; + SecComputer_Y.out.logic.ir_disagree = SecComputer_P.Constant_Value_l; + SecComputer_Y.out.logic.adr_computation_data.V_ias_kn = rtb_V_ias; + SecComputer_Y.out.logic.adr_computation_data.V_tas_kn = rtb_V_tas; + SecComputer_Y.out.logic.adr_computation_data.mach = rtb_mach; + SecComputer_Y.out.logic.adr_computation_data.alpha_deg = rtb_alpha; + SecComputer_Y.out.logic.ir_computation_data.theta_deg = rtb_theta; + SecComputer_Y.out.logic.ir_computation_data.phi_deg = rtb_phi; + SecComputer_Y.out.logic.ir_computation_data.q_deg_s = rtb_q; + SecComputer_Y.out.logic.ir_computation_data.r_deg_s = rtb_r; + SecComputer_Y.out.logic.ir_computation_data.n_x_g = rtb_n_x; + SecComputer_Y.out.logic.ir_computation_data.n_y_g = rtb_n_y; + SecComputer_Y.out.logic.ir_computation_data.n_z_g = rtb_n_z; + SecComputer_Y.out.logic.ir_computation_data.theta_dot_deg_s = rtb_theta_dot; + SecComputer_Y.out.logic.ir_computation_data.phi_dot_deg_s = rtb_phi_dot; + SecComputer_Y.out.logic.any_landing_gear_not_uplocked = rtb_OR; + SecComputer_Y.out.logic.lgciu_uplock_disagree_or_fault = rtb_OR6; + SecComputer_Y.out.discrete_outputs.thr_reverse_selected = SecComputer_P.Constant1_Value_g; + SecComputer_Y.out.discrete_outputs.left_elevator_ok = leftElevatorAvail; + SecComputer_Y.out.discrete_outputs.right_elevator_ok = rightElevatorAvail; + SecComputer_Y.out.discrete_outputs.ground_spoiler_out = SecComputer_DWork.Delay1_DSTATE_i; + SecComputer_Y.out.discrete_outputs.sec_failed = SecComputer_P.Constant2_Value_n; + SecComputer_Y.out.discrete_outputs.left_elevator_damping_mode = (rtb_isEngagedInPitch && leftElevatorAvail); + SecComputer_Y.out.discrete_outputs.right_elevator_damping_mode = (rtb_isEngagedInPitch && rightElevatorAvail); + SecComputer_Y.out.discrete_outputs.ths_active = (rtb_y_kz && rtb_thsAvail); + rtb_y_gp = (rtb_isEngagedInPitch || rtb_NOT_bl); + if (rtb_y_gp && leftElevatorAvail) { + SecComputer_Y.out.analog_outputs.left_elev_pos_order_deg = rtb_handleIndex; + } else { + SecComputer_Y.out.analog_outputs.left_elev_pos_order_deg = SecComputer_P.Constant_Value_h; + } + + if (rtb_y_gp && rightElevatorAvail) { + SecComputer_Y.out.analog_outputs.right_elev_pos_order_deg = rtb_handleIndex; + } else { + SecComputer_Y.out.analog_outputs.right_elev_pos_order_deg = SecComputer_P.Constant_Value_h; + } + + if (rtb_y_kz && rtb_thsAvail) { + SecComputer_Y.out.analog_outputs.ths_pos_order_deg = SecComputer_DWork.Delay_DSTATE; + } else { + SecComputer_Y.out.analog_outputs.ths_pos_order_deg = SecComputer_P.Constant_Value_h; + } + + if (spoilerPair1SupplyAvail) { + SecComputer_Y.out.analog_outputs.left_spoiler_1_pos_order_deg = rtb_Switch3; + SecComputer_Y.out.analog_outputs.right_spoiler_1_pos_order_deg = rtb_zeta_deg; + } else { + SecComputer_Y.out.analog_outputs.left_spoiler_1_pos_order_deg = SecComputer_P.Constant2_Value_f; + SecComputer_Y.out.analog_outputs.right_spoiler_1_pos_order_deg = SecComputer_P.Constant2_Value_f; + } + + if (spoiler_pair_2_avail) { + SecComputer_Y.out.analog_outputs.left_spoiler_2_pos_order_deg = pair1RollCommand; + SecComputer_Y.out.analog_outputs.right_spoiler_2_pos_order_deg = u0; + } else { + SecComputer_Y.out.analog_outputs.left_spoiler_2_pos_order_deg = SecComputer_P.Constant2_Value_f; + SecComputer_Y.out.analog_outputs.right_spoiler_2_pos_order_deg = SecComputer_P.Constant2_Value_f; + } + + if (SecComputer_U.in.discrete_inputs.l_spoiler_1_servo_failed) { + SecComputer_Y.out.bus_outputs.left_spoiler_1_position_deg.SSM = static_cast + (SecComputer_P.EnumeratedConstant_Value); + SecComputer_Y.out.bus_outputs.left_spoiler_1_position_deg.Data = static_cast + (SecComputer_P.Constant_Value_j); + } else { + SecComputer_Y.out.bus_outputs.left_spoiler_1_position_deg.SSM = static_cast + (SecComputer_P.EnumeratedConstant1_Value); + SecComputer_Y.out.bus_outputs.left_spoiler_1_position_deg.Data = static_cast + (SecComputer_U.in.analog_inputs.left_spoiler_1_pos_deg); + } + + if (SecComputer_U.in.discrete_inputs.r_spoiler_1_servo_failed) { + SecComputer_Y.out.bus_outputs.right_spoiler_1_position_deg.SSM = static_cast + (SecComputer_P.EnumeratedConstant_Value); + SecComputer_Y.out.bus_outputs.right_spoiler_1_position_deg.Data = static_cast + (SecComputer_P.Constant1_Value_d); + } else { + SecComputer_Y.out.bus_outputs.right_spoiler_1_position_deg.SSM = static_cast + (SecComputer_P.EnumeratedConstant1_Value); + SecComputer_Y.out.bus_outputs.right_spoiler_1_position_deg.Data = static_cast + (SecComputer_U.in.analog_inputs.right_spoiler_1_pos_deg); + } + + if (SecComputer_U.in.discrete_inputs.l_spoiler_2_servo_failed) { + SecComputer_Y.out.bus_outputs.left_spoiler_2_position_deg.SSM = static_cast + (SecComputer_P.EnumeratedConstant_Value); + SecComputer_Y.out.bus_outputs.left_spoiler_2_position_deg.Data = static_cast + (SecComputer_P.Constant5_Value_m); + } else { + SecComputer_Y.out.bus_outputs.left_spoiler_2_position_deg.SSM = static_cast + (SecComputer_P.EnumeratedConstant1_Value); + SecComputer_Y.out.bus_outputs.left_spoiler_2_position_deg.Data = static_cast + (SecComputer_U.in.analog_inputs.left_spoiler_2_pos_deg); + } + + if (SecComputer_U.in.discrete_inputs.r_spoiler_2_servo_failed) { + SecComputer_Y.out.bus_outputs.right_spoiler_2_position_deg.SSM = static_cast + (SecComputer_P.EnumeratedConstant_Value); + SecComputer_Y.out.bus_outputs.right_spoiler_2_position_deg.Data = static_cast + (SecComputer_P.Constant6_Value); + } else { + SecComputer_Y.out.bus_outputs.right_spoiler_2_position_deg.SSM = static_cast + (SecComputer_P.EnumeratedConstant1_Value); + SecComputer_Y.out.bus_outputs.right_spoiler_2_position_deg.Data = static_cast + (SecComputer_U.in.analog_inputs.right_spoiler_2_pos_deg); + } + + if (SecComputer_U.in.discrete_inputs.l_elev_servo_failed) { + SecComputer_Y.out.bus_outputs.left_elevator_position_deg.SSM = static_cast + (SecComputer_P.EnumeratedConstant_Value); + SecComputer_Y.out.bus_outputs.left_elevator_position_deg.Data = static_cast + (SecComputer_P.Constant2_Value_b); + } else { + SecComputer_Y.out.bus_outputs.left_elevator_position_deg.SSM = static_cast + (SecComputer_P.EnumeratedConstant1_Value); + SecComputer_Y.out.bus_outputs.left_elevator_position_deg.Data = static_cast + (SecComputer_U.in.analog_inputs.left_elevator_pos_deg); + } + + if (SecComputer_U.in.discrete_inputs.r_elev_servo_failed) { + SecComputer_Y.out.bus_outputs.right_elevator_position_deg.SSM = static_cast + (SecComputer_P.EnumeratedConstant_Value); + SecComputer_Y.out.bus_outputs.right_elevator_position_deg.Data = static_cast + (SecComputer_P.Constant3_Value_f); + } else { + SecComputer_Y.out.bus_outputs.right_elevator_position_deg.SSM = static_cast + (SecComputer_P.EnumeratedConstant1_Value); + SecComputer_Y.out.bus_outputs.right_elevator_position_deg.Data = static_cast + (SecComputer_U.in.analog_inputs.right_elevator_pos_deg); + } + + if (SecComputer_U.in.discrete_inputs.ths_motor_fault) { + SecComputer_Y.out.bus_outputs.ths_position_deg.SSM = static_cast(SecComputer_P.EnumeratedConstant_Value); + SecComputer_Y.out.bus_outputs.ths_position_deg.Data = static_cast(SecComputer_P.Constant4_Value_i); + } else { + SecComputer_Y.out.bus_outputs.ths_position_deg.SSM = static_cast(SecComputer_P.EnumeratedConstant1_Value); + SecComputer_Y.out.bus_outputs.ths_position_deg.Data = static_cast + (SecComputer_U.in.analog_inputs.ths_pos_deg); + } + + SecComputer_Y.out.bus_outputs.left_sidestick_pitch_command_deg.SSM = static_cast + (SecComputer_P.EnumeratedConstant1_Value); + SecComputer_Y.out.bus_outputs.left_sidestick_pitch_command_deg.Data = SecComputer_P.Gain_Gain_b * + static_cast(SecComputer_U.in.analog_inputs.capt_pitch_stick_pos); + SecComputer_Y.out.bus_outputs.right_sidestick_pitch_command_deg.SSM = static_cast + (SecComputer_P.EnumeratedConstant1_Value); + SecComputer_Y.out.bus_outputs.right_sidestick_pitch_command_deg.Data = SecComputer_P.Gain1_Gain * + static_cast(SecComputer_U.in.analog_inputs.fo_pitch_stick_pos); + SecComputer_Y.out.bus_outputs.left_sidestick_roll_command_deg.SSM = static_cast + (SecComputer_P.EnumeratedConstant1_Value); + SecComputer_Y.out.bus_outputs.left_sidestick_roll_command_deg.Data = SecComputer_P.Gain2_Gain * static_cast + (SecComputer_U.in.analog_inputs.capt_roll_stick_pos); + SecComputer_Y.out.bus_outputs.right_sidestick_roll_command_deg.SSM = static_cast + (SecComputer_P.EnumeratedConstant1_Value); + SecComputer_Y.out.bus_outputs.right_sidestick_roll_command_deg.Data = SecComputer_P.Gain3_Gain * + static_cast(SecComputer_U.in.analog_inputs.fo_roll_stick_pos); + SecComputer_Y.out.bus_outputs.speed_brake_lever_command_deg.SSM = static_cast + (SecComputer_P.EnumeratedConstant1_Value); + SecComputer_Y.out.bus_outputs.speed_brake_lever_command_deg.Data = SecComputer_P.Gain4_Gain * static_cast + (SecComputer_U.in.analog_inputs.spd_brk_lever_pos); + SecComputer_Y.out.bus_outputs.speed_brake_command_deg.SSM = static_cast + (SecComputer_P.EnumeratedConstant1_Value); + SecComputer_Y.out.bus_outputs.speed_brake_command_deg.Data = static_cast(rtb_Switch5); + SecComputer_Y.out.bus_outputs.thrust_lever_angle_1_deg.SSM = static_cast + (SecComputer_P.EnumeratedConstant1_Value); + SecComputer_Y.out.bus_outputs.thrust_lever_angle_1_deg.Data = static_cast + (SecComputer_U.in.analog_inputs.thr_lever_1_pos); + SecComputer_Y.out.bus_outputs.thrust_lever_angle_2_deg.SSM = static_cast + (SecComputer_P.EnumeratedConstant1_Value); + SecComputer_Y.out.bus_outputs.thrust_lever_angle_2_deg.Data = static_cast + (SecComputer_U.in.analog_inputs.thr_lever_2_pos); + SecComputer_Y.out.bus_outputs.discrete_status_word_1.SSM = static_cast + (SecComputer_P.EnumeratedConstant1_Value); + SecComputer_Y.out.bus_outputs.discrete_status_word_2.SSM = static_cast + (SecComputer_P.EnumeratedConstant1_Value); + SecComputer_DWork.Delay_DSTATE_c = rtb_y_p1; + SecComputer_DWork.Delay1_DSTATE = rtb_y_b; + SecComputer_DWork.icLoad = false; + SecComputer_DWork.Delay_DSTATE_l = SecComputer_DWork.Delay_DSTATE; + } else { + SecComputer_DWork.Runtime_MODE = false; + } +} + +void SecComputer::initialize() +{ + SecComputer_DWork.Delay_DSTATE_c = SecComputer_P.Delay_InitialCondition_c; + SecComputer_DWork.Delay1_DSTATE = SecComputer_P.Delay1_InitialCondition; + SecComputer_DWork.Memory_PreviousInput = SecComputer_P.SRFlipFlop_initial_condition; + SecComputer_DWork.Memory_PreviousInput_f = SecComputer_P.SRFlipFlop_initial_condition_c; + SecComputer_DWork.Memory_PreviousInput_n = SecComputer_P.SRFlipFlop_initial_condition_k; + SecComputer_DWork.Delay1_DSTATE_i = SecComputer_P.Delay1_InitialCondition_l; + SecComputer_DWork.Delay_DSTATE_n = SecComputer_P.Delay_InitialCondition_j; + SecComputer_DWork.Delay2_DSTATE = SecComputer_P.Delay2_InitialCondition; + SecComputer_DWork.Delay_DSTATE = SecComputer_P.Delay_InitialCondition; + SecComputer_DWork.icLoad = true; + LawMDLOBJ2.init(); + SecComputer_Y.out = SecComputer_P.out_Y0; +} + +void SecComputer::terminate() +{ +} + +SecComputer::SecComputer(): + SecComputer_U(), + SecComputer_Y(), + SecComputer_B(), + SecComputer_DWork() +{ +} + +SecComputer::~SecComputer() +{ +} diff --git a/hdw-a339x/src/wasm/fbw_a320/src/model/SecComputer_private.h b/hdw-a339x/src/wasm/fbw_a320/src/model/SecComputer_private.h new file mode 100644 index 000000000..8bd682d38 --- /dev/null +++ b/hdw-a339x/src/wasm/fbw_a320/src/model/SecComputer_private.h @@ -0,0 +1,6 @@ +#ifndef RTW_HEADER_SecComputer_private_h_ +#define RTW_HEADER_SecComputer_private_h_ +#include "rtwtypes.h" +#include "SecComputer_types.h" +#endif +