From 4b10ad4c650703ce587f1b6f4f5d493eb4c2cf8c Mon Sep 17 00:00:00 2001 From: lukecologne Date: Sun, 12 Nov 2023 02:02:47 +0100 Subject: [PATCH] feat(efcs): add precontrol to roll angle controller (#8272) * feat(efcs): add precontrol to roll rate controller * feat(fbw): increase default max simrate to 8, decrease min FPS to 5 * chore: add changelog * docs: also change max sim rate in provided example ini files --- .github/CHANGELOG.md | 2 + .../docs/Configuration/ModelConfiguration.ini | 2 +- .../wasm/fbw_a320/src/FlyByWireInterface.cpp | 22 +- .../wasm/fbw_a320/src/FlyByWireInterface.h | 2 +- .../src/interface/SimConnectInterface.cpp | 2 +- .../fbw_a320/src/model/LateralNormalLaw.cpp | 341 +++++++++--------- .../fbw_a320/src/model/LateralNormalLaw.h | 9 +- 7 files changed, 189 insertions(+), 191 deletions(-) diff --git a/.github/CHANGELOG.md b/.github/CHANGELOG.md index 8d87cd27f59..8e54796f76b 100644 --- a/.github/CHANGELOG.md +++ b/.github/CHANGELOG.md @@ -16,6 +16,8 @@ 1. [MCDU] Dash alternate time predictions if alternate fuel is manually inserted @BravoMike99 (bruno_pt99) 1. [BLEED] Add Air starter unit to enable ground starts - @Maximilian-Reuter (\_Chaoz_) 1. [MCDU] FUEL PRED layout improvemnts @BravoMike99 (bruno_pt99) +1. [EFCS] Add precontrol to roll angle controller to improve dynamic response - @lukecologne (luke) +1. [EFCS] Increase maximum sim rate limit to 8 due to improvements in roll law stability - @lukecologne (luke) ## 0.11.0 diff --git a/fbw-a32nx/docs/Configuration/ModelConfiguration.ini b/fbw-a32nx/docs/Configuration/ModelConfiguration.ini index 28f13596a8f..f5a45e1894c 100644 --- a/fbw-a32nx/docs/Configuration/ModelConfiguration.ini +++ b/fbw-a32nx/docs/Configuration/ModelConfiguration.ini @@ -20,7 +20,7 @@ ; !! WARNING CHANGE AT YOUR OWN RISK !! ; maximum allowed simulation rate -;maximum_simulation_rate = 4 +;maximum_simulation_rate = 8 ; !! WARNING CHANGE AT YOUR OWN RISK !! ; if enabled, limit simulation rate by performance diff --git a/fbw-a32nx/src/wasm/fbw_a320/src/FlyByWireInterface.cpp b/fbw-a32nx/src/wasm/fbw_a320/src/FlyByWireInterface.cpp index 7193f86534d..26a109f6452 100644 --- a/fbw-a32nx/src/wasm/fbw_a320/src/FlyByWireInterface.cpp +++ b/fbw-a32nx/src/wasm/fbw_a320/src/FlyByWireInterface.cpp @@ -31,14 +31,12 @@ bool FlyByWireInterface::connect() { // initialize flight data recorder flightDataRecorder.initialize(); - - // connect to sim connect - bool success = simConnectInterface.connect(clientDataEnabled, autopilotStateMachineEnabled, autopilotLawsEnabled, flyByWireEnabled, elacDisabled, - secDisabled, facDisabled, throttleAxis, spoilersHandler, flightControlsKeyChangeAileron, - flightControlsKeyChangeElevator, flightControlsKeyChangeRudder, - disableXboxCompatibilityRudderAxisPlusMinus, enableRudder2AxisMode, idMinimumSimulationRate->get(), - idMaximumSimulationRate->get(), limitSimulationRateByPerformance); + bool success = simConnectInterface.connect( + clientDataEnabled, autopilotStateMachineEnabled, autopilotLawsEnabled, flyByWireEnabled, elacDisabled, secDisabled, facDisabled, + throttleAxis, spoilersHandler, flightControlsKeyChangeAileron, flightControlsKeyChangeElevator, flightControlsKeyChangeRudder, + disableXboxCompatibilityRudderAxisPlusMinus, enableRudder2AxisMode, idMinimumSimulationRate->get(), idMaximumSimulationRate->get(), + limitSimulationRateByPerformance); // request data if (!simConnectInterface.requestData()) { @@ -207,7 +205,7 @@ void FlyByWireInterface::loadConfiguration() { // -------------------------------------------------------------------------- // load values - autopilot idMinimumSimulationRate->set(INITypeConversion::getDouble(iniStructure, "AUTOPILOT", "MINIMUM_SIMULATION_RATE", 1)); - idMaximumSimulationRate->set(INITypeConversion::getDouble(iniStructure, "AUTOPILOT", "MAXIMUM_SIMULATION_RATE", 4)); + idMaximumSimulationRate->set(INITypeConversion::getDouble(iniStructure, "AUTOPILOT", "MAXIMUM_SIMULATION_RATE", 8)); limitSimulationRateByPerformance = INITypeConversion::getBoolean(iniStructure, "AUTOPILOT", "LIMIT_SIMULATION_RATE_BY_PERFORMANCE", true); simulationRateReductionEnabled = INITypeConversion::getBoolean(iniStructure, "AUTOPILOT", "SIMULATION_RATE_REDUCTION_ENABLED", true); @@ -807,8 +805,6 @@ bool FlyByWireInterface::readDataAndLocalVariables(double sampleTime) { simConnectInterface.setLoggingFlightControlsEnabled(idLoggingFlightControlsEnabled->get() == 1); simConnectInterface.setLoggingThrottlesEnabled(idLoggingThrottlesEnabled->get() == 1); - - // read data if (!simConnectInterface.readData()) { std::cout << "WASM: Read data failed!" << std::endl; @@ -1332,7 +1328,8 @@ bool FlyByWireInterface::updateElac(double sampleTime, int elacIndex) { if (elacIndex == 0) { powerSupplyAvailable = idElecDcEssBusPowered->get() || - ((elacsDiscreteOutputs[0].batt_power_supply || secsDiscreteOutputs[0].batt_power_supply) ? idElecBat1HotBusPowered->get() : false); + ((elacsDiscreteOutputs[0].batt_power_supply || secsDiscreteOutputs[0].batt_power_supply) ? idElecBat1HotBusPowered->get() + : false); } else { bool elac1OrSec1PowersupplySwitched = elacsDiscreteOutputs[0].batt_power_supply || secsDiscreteOutputs[0].batt_power_supply; bool elac2NormalSupplyAvail = idElecDcBus2Powered->get(); @@ -1504,7 +1501,8 @@ bool FlyByWireInterface::updateSec(double sampleTime, int secIndex) { if (secIndex == 0) { powerSupplyAvailable = idElecDcEssBusPowered->get() || - ((secsDiscreteOutputs[0].batt_power_supply || elacsDiscreteOutputs[0].batt_power_supply) ? idElecBat1HotBusPowered->get() : false); + ((secsDiscreteOutputs[0].batt_power_supply || elacsDiscreteOutputs[0].batt_power_supply) ? idElecBat1HotBusPowered->get() + : false); } else { powerSupplyAvailable = idElecDcBus2Powered->get(); } diff --git a/fbw-a32nx/src/wasm/fbw_a320/src/FlyByWireInterface.h b/fbw-a32nx/src/wasm/fbw_a320/src/FlyByWireInterface.h index 9b4ef4f6532..2ef1dadb30e 100644 --- a/fbw-a32nx/src/wasm/fbw_a320/src/FlyByWireInterface.h +++ b/fbw-a32nx/src/wasm/fbw_a320/src/FlyByWireInterface.h @@ -38,7 +38,7 @@ class FlyByWireInterface { private: const std::string CONFIGURATION_FILEPATH = "\\work\\ModelConfiguration.ini"; - static constexpr double MAX_ACCEPTABLE_SAMPLE_TIME = 0.11; + static constexpr double MAX_ACCEPTABLE_SAMPLE_TIME = 0.22; static constexpr uint32_t LOW_PERFORMANCE_TIMER_THRESHOLD = 10; uint32_t lowPerformanceTimer = 0; diff --git a/fbw-a32nx/src/wasm/fbw_a320/src/interface/SimConnectInterface.cpp b/fbw-a32nx/src/wasm/fbw_a320/src/interface/SimConnectInterface.cpp index c630a3dbe61..d4833523db9 100644 --- a/fbw-a32nx/src/wasm/fbw_a320/src/interface/SimConnectInterface.cpp +++ b/fbw-a32nx/src/wasm/fbw_a320/src/interface/SimConnectInterface.cpp @@ -2699,7 +2699,7 @@ void SimConnectInterface::simConnectProcessEvent(const SIMCONNECT_RECV_EVENT* ev // calculate frame rate that will be seen by FBW / AP double theoreticalFrameRate = (1 / sampleTime) / (simData.simulation_rate * 2); // determine if an increase of simulation rate can be allowed - if ((simData.simulation_rate < maxSimulationRate && theoreticalFrameRate >= 8) || simData.simulation_rate < 1 || + if ((simData.simulation_rate < maxSimulationRate && theoreticalFrameRate >= 5) || simData.simulation_rate < 1 || !limitSimulationRateByPerformance) { sendEvent(Events::SIM_RATE_INCR, 0, SIMCONNECT_GROUP_PRIORITY_DEFAULT); std::cout << "WASM: Simulation rate " << simData.simulation_rate; diff --git a/fbw-a32nx/src/wasm/fbw_a320/src/model/LateralNormalLaw.cpp b/fbw-a32nx/src/wasm/fbw_a320/src/model/LateralNormalLaw.cpp index 590d943e722..05e842cabb1 100644 --- a/fbw-a32nx/src/wasm/fbw_a320/src/model/LateralNormalLaw.cpp +++ b/fbw-a32nx/src/wasm/fbw_a320/src/model/LateralNormalLaw.cpp @@ -34,6 +34,8 @@ LateralNormalLaw::Parameters_LateralNormalLaw_T LateralNormalLaw::LateralNormalL 1.0, + 1.0, + 0.0, 0.0, @@ -72,8 +74,6 @@ LateralNormalLaw::Parameters_LateralNormalLaw_T LateralNormalLaw::LateralNormalL -5.0, - -15.0, - -1000.0, -2.0, @@ -84,8 +84,6 @@ LateralNormalLaw::Parameters_LateralNormalLaw_T LateralNormalLaw::LateralNormalL 5.0, - 15.0, - 0.33333333333333331, 2.0, @@ -140,10 +138,6 @@ LateralNormalLaw::Parameters_LateralNormalLaw_T LateralNormalLaw::LateralNormalL 0.0, - 67.0, - - -67.0, - 9.81, 0.017453292519943295, @@ -239,19 +233,20 @@ void LateralNormalLaw::LateralNormalLaw_LagFilter(real_T rtu_U, real_T rtu_C1, c void LateralNormalLaw::init(void) { - LateralNormalLaw_DWork.Delay_DSTATE = LateralNormalLaw_rtP.Delay_InitialCondition; + LateralNormalLaw_DWork.Delay_DSTATE = LateralNormalLaw_rtP.DiscreteDerivativeVariableTs_InitialCondition; + LateralNormalLaw_DWork.Delay_DSTATE_e = LateralNormalLaw_rtP.Delay_InitialCondition; LateralNormalLaw_DWork.icLoad = true; } void LateralNormalLaw::reset(void) { - LateralNormalLaw_DWork.Delay_DSTATE = LateralNormalLaw_rtP.Delay_InitialCondition; + LateralNormalLaw_DWork.Delay_DSTATE = LateralNormalLaw_rtP.DiscreteDerivativeVariableTs_InitialCondition; + LateralNormalLaw_DWork.Delay_DSTATE_e = 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_h = false; @@ -266,25 +261,31 @@ void LateralNormalLaw::step(const real_T *rtu_In_time_dt, const real_T *rtu_In_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 real_T c_0[4]{ 1.0, 1.2, 2.0, 2.0 }; + 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 L_xi; real_T Vias; - real_T Vtas; + real_T b_x; + real_T k_phi; real_T r; - real_T rtb_Gain1; - real_T rtb_Gain1_c; - real_T rtb_Gain1_l; + real_T rtb_Divide; + real_T rtb_Gain; real_T rtb_Gain_b; - real_T rtb_Sum_x0; + real_T rtb_Saturation2; + real_T rtb_Saturation_f; + real_T rtb_Switch2; real_T rtb_Y_i; - real_T rtb_Y_j; + real_T rtb_Y_o; real_T rtb_beDot; + real_T rtb_input_p_V_tas_kn; + real_T v_cas_ms; + real_T x; int32_T low_i; int32_T low_ip1; int32_T mid_i; @@ -320,10 +321,10 @@ void LateralNormalLaw::step(const real_T *rtu_In_time_dt, const real_T *rtu_In_T LateralNormalLaw_rtP.RateLimiterVariableTs_lo, rtu_In_time_dt, LateralNormalLaw_rtP.RateLimiterVariableTs_InitialCondition, &rtb_Y_i, &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; + v_cas_ms = *rtu_In_V_ias_kn; + rtb_Gain = *rtu_In_delta_zeta_pos; + Vias = std::fmax(v_cas_ms, 60.0); + v_cas_ms = 0.0; if (Vias <= 380.0) { rtb_in_flight = 4; low_i = 1; @@ -340,112 +341,140 @@ void LateralNormalLaw::step(const real_T *rtu_In_time_dt, const real_T *rtu_In_T r = (Vias - static_cast(b[low_i - 1])) / static_cast(b[low_i] - b[low_i - 1]); if (r == 0.0) { - r = -15.0; + v_cas_ms = -15.0; } else if (r == 1.0) { - r = c[low_i]; + v_cas_ms = c[low_i]; } else if (c[low_i] == -15) { - r = -15.0; + v_cas_ms = -15.0; } else { - r = (1.0 - r) * -15.0 + r * static_cast(c[low_i]); + v_cas_ms = (1.0 - r) * -15.0 + r * static_cast(c[low_i]); } } - Vias *= 0.5144; - LateralNormalLaw_RateLimiter(0.814 / std::sqrt(1.3734E+6 / (149.45000000000002 * (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_Saturation_f = Vias * 0.5144; + LateralNormalLaw_RateLimiter(0.814 / std::sqrt(1.3734E+6 / (149.45000000000002 * (rtb_Saturation_f * rtb_Saturation_f))) + * (v_cas_ms * rtb_Gain), LateralNormalLaw_rtP.RateLimiterVariableTs1_up, + LateralNormalLaw_rtP.RateLimiterVariableTs1_lo, rtu_In_time_dt, + LateralNormalLaw_rtP.RateLimiterVariableTs1_InitialCondition, &Vias, &LateralNormalLaw_DWork.sf_RateLimiter_d); + rtb_Y_o = *rtu_In_r_deg_s; + rtb_Switch2 = *rtu_In_V_ias_kn; + rtb_input_p_V_tas_kn = *rtu_In_V_tas_kn; rtb_beDot = *rtu_In_delta_zeta_pos; - rtb_Gain1 = LateralNormalLaw_rtP.Gain1_Gain * *rtu_In_delta_xi_pos; + rtb_Saturation2 = LateralNormalLaw_rtP.Gain1_Gain * *rtu_In_delta_xi_pos; + rtb_Gain = LateralNormalLaw_rtP.DiscreteDerivativeVariableTs_Gain * rtb_Saturation2; + rtb_Divide = (rtb_Gain - LateralNormalLaw_DWork.Delay_DSTATE) / *rtu_In_time_dt; + v_cas_ms = std::fmax(*rtu_In_V_ias_kn, 80.0) * 0.5144; + Vias = v_cas_ms * v_cas_ms * 0.6125; + L_xi = Vias * 122.0 * 17.9 * -0.090320788790706555 / 1.0E+6; + r = 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) { + r = c_0[low_i]; + } else if (r == 1.0) { + r = c_0[low_i + 1]; + } else { + k_phi = c_0[low_i + 1]; + if (k_phi == c_0[low_i]) { + r = c_0[low_i]; + } else { + r = (1.0 - r) * c_0[low_i] + k_phi * r; + } + } + } + + k_phi = -(r * r) / L_xi; if (*rtu_In_high_speed_prot_active) { - Vias = look1_binlxpw(*rtu_In_Phi_deg, LateralNormalLaw_rtP.BankAngleProtection2_bp01Data, - LateralNormalLaw_rtP.BankAngleProtection2_tableData, 4U); + rtb_Saturation_f = 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); + rtb_Saturation_f = 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_Saturation_f = 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; + b_x = 15.0; + x = -15.0; + if (LateralNormalLaw_DWork.Delay_DSTATE_e >= 25.0) { + x = *rtu_In_pk_deg_s; + } else if (LateralNormalLaw_DWork.Delay_DSTATE_e <= -25.0) { + b_x = *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; + rtb_Saturation2 = (1.0 / r * rtb_Divide + rtb_Saturation2) + rtb_Saturation_f; + if (rtb_Saturation2 > LateralNormalLaw_rtP.Saturation_UpperSat_a) { + rtb_Saturation2 = LateralNormalLaw_rtP.Saturation_UpperSat_a; + } else if (rtb_Saturation2 < LateralNormalLaw_rtP.Saturation_LowerSat_o) { + rtb_Saturation2 = LateralNormalLaw_rtP.Saturation_LowerSat_o; } - Vias = std::fmin(rtb_Sum_x0, std::fmax(rtb_Gain1_l, rtb_Gain1 * rtb_Y_i)) * + rtb_Saturation_f = std::fmin(b_x, std::fmax(x, rtb_Saturation2 * rtb_Y_i)) * LateralNormalLaw_rtP.DiscreteTimeIntegratorVariableTs_Gain * *rtu_In_time_dt; rtb_OR = ((rtb_Y_i == 0.0) || (*rtu_In_tracking_mode_on) || (*rtu_In_any_ap_engaged)); - rtb_Sum_x0 = *rtu_In_Phi_deg - Vias; + rtb_Divide = *rtu_In_Phi_deg - rtb_Saturation_f; 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 = rtb_Divide; } - LateralNormalLaw_DWork.Delay_DSTATE_d += Vias; + LateralNormalLaw_DWork.Delay_DSTATE_d += rtb_Saturation_f; 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; + rtb_Divide = *rtu_In_Phi_deg; } else { - rtb_Sum_x0 = *rtu_In_ap_phi_c_deg; + rtb_Divide = *rtu_In_ap_phi_c_deg; } + } else { + rtb_Divide = LateralNormalLaw_DWork.Delay_DSTATE_d; } - 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 * 122.0 / (70000.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; + rtb_Saturation2 = std::fmax(rtb_input_p_V_tas_kn * 0.5144, 60.0); + rtb_Saturation_f = rtb_Switch2 * 0.5144; + if (rtb_Switch2 >= 60.0) { + rtb_beDot = (rtb_Saturation_f * rtb_Saturation_f * 0.6125 * 122.0 / (70000.0 * rtb_Saturation2) * 3.172 * -rtb_beDot + * 3.1415926535897931 / 180.0 + (rtb_Divide * 3.1415926535897931 / 180.0 * (9.81 / rtb_Saturation2) + + -(rtb_Y_o * 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_LagFilter(rtb_beDot, LateralNormalLaw_rtP.LagFilter_C1, rtu_In_time_dt, &rtb_Y_o, &LateralNormalLaw_DWork.sf_LagFilter); - Vias = look1_binlxpw(*rtu_In_V_ias_kn, LateralNormalLaw_rtP.ScheduledGain2_BreakpointsForDimension1, - LateralNormalLaw_rtP.ScheduledGain2_Table, 3U); + rtb_Saturation_f = 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; + rtb_Switch2 = *rtu_In_ap_beta_c_deg; } else { - rtb_Y_j = *rtu_In_delta_zeta_pos * Vias; + rtb_Switch2 = *rtu_In_delta_zeta_pos * rtb_Saturation_f; } - 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); + rtb_Saturation_f = look1_binlxpw(*rtu_In_V_ias_kn, LateralNormalLaw_rtP.ScheduledGain1_BreakpointsForDimension1, + LateralNormalLaw_rtP.ScheduledGain1_Table, 4U); + LateralNormalLaw_LagFilter((rtb_Switch2 - rtb_Y_o) * rtb_Saturation_f - rtb_beDot, LateralNormalLaw_rtP.LagFilter_C1_d, + rtu_In_time_dt, &rtb_Y_o, &LateralNormalLaw_DWork.sf_LagFilter_m); + rtb_Saturation2 = look1_binlxpw(*rtu_In_V_ias_kn, LateralNormalLaw_rtP.ScheduledGain_BreakpointsForDimension1, + LateralNormalLaw_rtP.ScheduledGain_Table, 8U); if (!LateralNormalLaw_DWork.pY_not_empty_h) { LateralNormalLaw_DWork.pY_p = LateralNormalLaw_rtP.RateLimiterVariableTs_InitialCondition_d; LateralNormalLaw_DWork.pY_not_empty_h = true; @@ -456,39 +485,40 @@ void LateralNormalLaw::step(const real_T *rtu_In_time_dt, const real_T *rtu_In_T (LateralNormalLaw_rtP.RateLimiterVariableTs_lo_l) * *rtu_In_time_dt); if (*rtu_In_any_ap_engaged) { if (LateralNormalLaw_DWork.pY_p > LateralNormalLaw_rtP.Saturation_UpperSat) { - Vias = LateralNormalLaw_rtP.Saturation_UpperSat; + rtb_Saturation_f = LateralNormalLaw_rtP.Saturation_UpperSat; } else if (LateralNormalLaw_DWork.pY_p < LateralNormalLaw_rtP.Saturation_LowerSat) { - Vias = LateralNormalLaw_rtP.Saturation_LowerSat; + rtb_Saturation_f = LateralNormalLaw_rtP.Saturation_LowerSat; } else { - Vias = LateralNormalLaw_DWork.pY_p; + rtb_Saturation_f = LateralNormalLaw_DWork.pY_p; } - 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; + rtb_beDot = *rtu_In_ap_beta_c_deg * rtb_Saturation_f; + rtb_Saturation2 = rtb_Switch2 * rtb_Saturation2 + rtb_Y_o; + if (rtb_Saturation2 > LateralNormalLaw_rtP.Saturation_UpperSat_f) { + rtb_Saturation2 = LateralNormalLaw_rtP.Saturation_UpperSat_f; + } else if (rtb_Saturation2 < LateralNormalLaw_rtP.Saturation_LowerSat_j) { + rtb_Saturation2 = LateralNormalLaw_rtP.Saturation_LowerSat_j; } - Vias = (LateralNormalLaw_rtP.Constant_Value - Vias) * rtb_Gain1 + rtb_beDot; + rtb_Saturation_f = (LateralNormalLaw_rtP.Constant_Value - rtb_Saturation_f) * rtb_Saturation2 + rtb_beDot; } else { - Vias = LateralNormalLaw_rtP.Constant_Value_b; + rtb_Saturation_f = 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; + rtb_Y_o = LateralNormalLaw_rtP.Gain1_Gain_l * *rtu_In_Theta_deg; + rtb_Saturation2 = *rtu_In_V_tas_kn; + if (rtb_Saturation2 > LateralNormalLaw_rtP.Saturation_UpperSat_e) { + rtb_Saturation2 = LateralNormalLaw_rtP.Saturation_UpperSat_e; + } else if (rtb_Saturation2 < LateralNormalLaw_rtP.Saturation_LowerSat_jd) { + rtb_Saturation2 = 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, + rtb_Y_o = *rtu_In_r_deg_s - std::sin(LateralNormalLaw_rtP.Gain1_Gain_f * rtb_Divide) * + LateralNormalLaw_rtP.Constant2_Value * std::cos(rtb_Y_o) / (LateralNormalLaw_rtP.Gain6_Gain * rtb_Saturation2) * + LateralNormalLaw_rtP.Gain_Gain_i; + rtb_Saturation2 = 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_Switch2 = rtb_Y_o * rtb_Saturation2; rtb_OR = !*rtu_In_on_ground; if (!LateralNormalLaw_DWork.pY_not_empty) { LateralNormalLaw_DWork.pY = LateralNormalLaw_rtP.RateLimiterVariableTs1_InitialCondition_m; @@ -499,67 +529,37 @@ void LateralNormalLaw::step(const real_T *rtu_In_time_dt, const real_T *rtu_In_T (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; + rtb_Y_o = LateralNormalLaw_rtP.Saturation_UpperSat_n; } else if (LateralNormalLaw_DWork.pY < LateralNormalLaw_rtP.Saturation_LowerSat_b) { - r = LateralNormalLaw_rtP.Saturation_LowerSat_b; + rtb_Y_o = LateralNormalLaw_rtP.Saturation_LowerSat_b; } else { - r = LateralNormalLaw_DWork.pY; + rtb_Y_o = LateralNormalLaw_DWork.pY; } - rtb_Y_j = look1_binlxpw(*rtu_In_V_tas_kn, LateralNormalLaw_rtP.ScheduledGain1_BreakpointsForDimension1_j, + rtb_Saturation2 = 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; + rtb_Saturation2 *= *rtu_In_r_deg_s; + if (rtb_Switch2 > LateralNormalLaw_rtP.Saturation1_UpperSat) { + rtb_Switch2 = LateralNormalLaw_rtP.Saturation1_UpperSat; + } else if (rtb_Switch2 < LateralNormalLaw_rtP.Saturation1_LowerSat) { + rtb_Switch2 = LateralNormalLaw_rtP.Saturation1_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 * 122.0 * 17.9 * -0.090320788790706555 / 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; - } + if (rtb_Saturation2 > LateralNormalLaw_rtP.Saturation2_UpperSat) { + rtb_Saturation2 = LateralNormalLaw_rtP.Saturation2_UpperSat; + } else if (rtb_Saturation2 < LateralNormalLaw_rtP.Saturation2_LowerSat) { + rtb_Saturation2 = LateralNormalLaw_rtP.Saturation2_LowerSat; } - 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 * 122.0 * 320.40999999999997 * -0.487 / 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; + *rty_Out_zeta_deg = ((LateralNormalLaw_rtP.Constant_Value_k - rtb_Y_o) * rtb_Saturation2 + rtb_Switch2 * rtb_Y_o) + + rtb_Saturation_f; + rtb_Saturation_f = LateralNormalLaw_rtP.Gain1_Gain_b * *rtu_In_Phi_deg; + rtb_Switch2 = LateralNormalLaw_rtP.Gain1_Gain_c * *rtu_In_pk_deg_s; + rtb_Y_o = look1_binlxpw(*rtu_In_time_dt, LateralNormalLaw_rtP.ScheduledGain_BreakpointsForDimension1_j, + LateralNormalLaw_rtP.ScheduledGain_Table_i, 4U); + LateralNormalLaw_DWork.Delay_DSTATE_e = ((-(Vias / v_cas_ms * 122.0 * 320.40999999999997 * -0.487 / 1.0E+6 + 1.414 * r) + / L_xi * rtb_Switch2 + k_phi * rtb_Saturation_f) + LateralNormalLaw_rtP.Gain1_Gain_n * rtb_Divide * -k_phi) * + rtb_Y_o * LateralNormalLaw_rtP.Gain_Gain_p; if (rtb_Y_i > LateralNormalLaw_rtP.Saturation1_UpperSat_e) { rtb_Y_i = LateralNormalLaw_rtP.Saturation1_UpperSat_e; } else if (rtb_Y_i < LateralNormalLaw_rtP.Saturation1_LowerSat_l) { @@ -567,25 +567,26 @@ void LateralNormalLaw::step(const real_T *rtu_In_time_dt, const real_T *rtu_In_T } if (rtb_Y_i > LateralNormalLaw_rtP.Saturation_UpperSat_l) { - rtb_Y_j = LateralNormalLaw_rtP.Saturation_UpperSat_l; + rtb_Saturation_f = LateralNormalLaw_rtP.Saturation_UpperSat_l; } else if (rtb_Y_i < LateralNormalLaw_rtP.Saturation_LowerSat_og) { - rtb_Y_j = LateralNormalLaw_rtP.Saturation_LowerSat_og; + rtb_Saturation_f = LateralNormalLaw_rtP.Saturation_LowerSat_og; } else { - rtb_Y_j = rtb_Y_i; + rtb_Saturation_f = rtb_Y_i; } - if (LateralNormalLaw_DWork.Delay_DSTATE > LateralNormalLaw_rtP.Limiterxi_UpperSat) { - rtb_Y_i = LateralNormalLaw_rtP.Limiterxi_UpperSat; - } else if (LateralNormalLaw_DWork.Delay_DSTATE < LateralNormalLaw_rtP.Limiterxi_LowerSat) { - rtb_Y_i = LateralNormalLaw_rtP.Limiterxi_LowerSat; + if (LateralNormalLaw_DWork.Delay_DSTATE_e > LateralNormalLaw_rtP.Limiterxi_UpperSat) { + k_phi = LateralNormalLaw_rtP.Limiterxi_UpperSat; + } else if (LateralNormalLaw_DWork.Delay_DSTATE_e < LateralNormalLaw_rtP.Limiterxi_LowerSat) { + k_phi = LateralNormalLaw_rtP.Limiterxi_LowerSat; } else { - rtb_Y_i = LateralNormalLaw_DWork.Delay_DSTATE; + k_phi = LateralNormalLaw_DWork.Delay_DSTATE_e; } - LateralNormalLaw_RateLimiter(rtb_Y_i * 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_RateLimiter(k_phi * rtb_Saturation_f + (LateralNormalLaw_rtP.Constant_Value_l1 - rtb_Saturation_f) * + 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.Delay_DSTATE = rtb_Gain; LateralNormalLaw_DWork.icLoad = false; } diff --git a/fbw-a32nx/src/wasm/fbw_a320/src/model/LateralNormalLaw.h b/fbw-a32nx/src/wasm/fbw_a320/src/model/LateralNormalLaw.h index a533fd859ef..e0a3a381f71 100644 --- a/fbw-a32nx/src/wasm/fbw_a320/src/model/LateralNormalLaw.h +++ b/fbw-a32nx/src/wasm/fbw_a320/src/model/LateralNormalLaw.h @@ -21,6 +21,7 @@ class LateralNormalLaw final struct D_Work_LateralNormalLaw_T { real_T Delay_DSTATE; + real_T Delay_DSTATE_e; real_T Delay_DSTATE_d; real_T pY; real_T pY_p; @@ -32,7 +33,6 @@ class LateralNormalLaw final 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; }; @@ -46,10 +46,11 @@ class LateralNormalLaw final real_T ScheduledGain_BreakpointsForDimension1_j[5]; real_T LagFilter_C1; real_T LagFilter_C1_d; + real_T DiscreteDerivativeVariableTs_Gain; real_T DiscreteTimeIntegratorVariableTs_Gain; real_T RateLimiterVariableTs_InitialCondition; real_T RateLimiterVariableTs1_InitialCondition; - real_T RateLimiterVariableTs_InitialCondition_m; + real_T DiscreteDerivativeVariableTs_InitialCondition; real_T RateLimiterVariableTs_InitialCondition_d; real_T RateLimiterVariableTs1_InitialCondition_m; real_T RateLimiterVariableTs_InitialCondition_k; @@ -63,13 +64,11 @@ class LateralNormalLaw final 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; @@ -94,8 +93,6 @@ class LateralNormalLaw final 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;