From 69e937430403e8acf4be52d2c3bd0ee482e0f7a9 Mon Sep 17 00:00:00 2001 From: kevinwidjaja21 Date: Sun, 12 Nov 2023 23:30:48 +0800 Subject: [PATCH] Fix lateral law --- .../src/model/LateralDirectLaw_private.h | 6 - .../fbw_a320/src/model/LateralNormalLaw.cpp | 56 +++---- .../fbw_a320/src/model/LateralNormalLaw.h | 155 ------------------ .../src/model/LateralNormalLaw_private.h | 6 - 4 files changed, 28 insertions(+), 195 deletions(-) delete mode 100644 hdw-a339x/src/wasm/fbw_a320/src/model/LateralDirectLaw_private.h delete mode 100644 hdw-a339x/src/wasm/fbw_a320/src/model/LateralNormalLaw.h delete mode 100644 hdw-a339x/src/wasm/fbw_a320/src/model/LateralNormalLaw_private.h 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 deleted file mode 100644 index 5d1ddf24a..000000000 --- a/hdw-a339x/src/wasm/fbw_a320/src/model/LateralDirectLaw_private.h +++ /dev/null @@ -1,6 +0,0 @@ -#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 index c52750441..5df5753fc 100644 --- a/hdw-a339x/src/wasm/fbw_a320/src/model/LateralNormalLaw.cpp +++ b/hdw-a339x/src/wasm/fbw_a320/src/model/LateralNormalLaw.cpp @@ -254,7 +254,7 @@ void LateralNormalLaw::reset(void) 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_h = false; LateralNormalLaw_DWork.pY_not_empty = false; LateralNormalLaw_RateLimiter_Reset(&LateralNormalLaw_DWork.sf_RateLimiter_j); } @@ -282,8 +282,8 @@ void LateralNormalLaw::step(const real_T *rtu_In_time_dt, const real_T *rtu_In_T real_T rtb_Gain1_l; real_T rtb_Gain_b; real_T rtb_Sum_x0; + real_T rtb_Y_i; real_T rtb_Y_j; - real_T rtb_Y_p; real_T rtb_beDot; int32_T low_i; int32_T low_ip1; @@ -309,16 +309,16 @@ void LateralNormalLaw::step(const real_T *rtu_In_time_dt, const real_T *rtu_In_T } if (rtb_in_flight > LateralNormalLaw_rtP.Saturation_UpperSat_p) { - Vias = LateralNormalLaw_rtP.Saturation_UpperSat_p; + rtb_Gain_b = LateralNormalLaw_rtP.Saturation_UpperSat_p; } else if (rtb_in_flight < LateralNormalLaw_rtP.Saturation_LowerSat_h) { - Vias = LateralNormalLaw_rtP.Saturation_LowerSat_h; + rtb_Gain_b = LateralNormalLaw_rtP.Saturation_LowerSat_h; } else { - Vias = rtb_in_flight; + rtb_Gain_b = rtb_in_flight; } - LateralNormalLaw_RateLimiter(Vias, LateralNormalLaw_rtP.RateLimiterVariableTs_up, + LateralNormalLaw_RateLimiter(rtb_Gain_b, LateralNormalLaw_rtP.RateLimiterVariableTs_up, LateralNormalLaw_rtP.RateLimiterVariableTs_lo, rtu_In_time_dt, - LateralNormalLaw_rtP.RateLimiterVariableTs_InitialCondition, &rtb_Y_p, &LateralNormalLaw_DWork.sf_RateLimiter); + 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; @@ -385,9 +385,9 @@ void LateralNormalLaw::step(const real_T *rtu_In_time_dt, const real_T *rtu_In_T rtb_Gain1 = LateralNormalLaw_rtP.Saturation_LowerSat_o; } - Vias = std::fmin(rtb_Sum_x0, std::fmax(rtb_Gain1_l, rtb_Gain1 * rtb_Y_p)) * + Vias = std::fmin(rtb_Sum_x0, std::fmax(rtb_Gain1_l, rtb_Gain1 * rtb_Y_i)) * 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_OR = ((rtb_Y_i == 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) { @@ -446,21 +446,21 @@ void LateralNormalLaw::step(const real_T *rtu_In_time_dt, const real_T *rtu_In_T &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; + if (!LateralNormalLaw_DWork.pY_not_empty_h) { + LateralNormalLaw_DWork.pY_p = LateralNormalLaw_rtP.RateLimiterVariableTs_InitialCondition_d; + LateralNormalLaw_DWork.pY_not_empty_h = 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_DWork.pY_p += std::fmax(std::fmin(static_cast(*rtu_In_on_ground) - + LateralNormalLaw_DWork.pY_p, 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) { + if (LateralNormalLaw_DWork.pY_p > LateralNormalLaw_rtP.Saturation_UpperSat) { Vias = LateralNormalLaw_rtP.Saturation_UpperSat; - } else if (LateralNormalLaw_DWork.pY_d < LateralNormalLaw_rtP.Saturation_LowerSat) { + } else if (LateralNormalLaw_DWork.pY_p < LateralNormalLaw_rtP.Saturation_LowerSat) { Vias = LateralNormalLaw_rtP.Saturation_LowerSat; } else { - Vias = LateralNormalLaw_DWork.pY_d; + Vias = LateralNormalLaw_DWork.pY_p; } rtb_beDot = *rtu_In_ap_beta_c_deg * Vias; @@ -560,29 +560,29 @@ void LateralNormalLaw::step(const real_T *rtu_In_time_dt, const real_T *rtu_In_T 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_i > LateralNormalLaw_rtP.Saturation1_UpperSat_e) { + rtb_Y_i = LateralNormalLaw_rtP.Saturation1_UpperSat_e; + } else if (rtb_Y_i < LateralNormalLaw_rtP.Saturation1_LowerSat_l) { + rtb_Y_i = LateralNormalLaw_rtP.Saturation1_LowerSat_l; } - if (rtb_Y_p > LateralNormalLaw_rtP.Saturation_UpperSat_l) { + if (rtb_Y_i > LateralNormalLaw_rtP.Saturation_UpperSat_l) { rtb_Y_j = LateralNormalLaw_rtP.Saturation_UpperSat_l; - } else if (rtb_Y_p < LateralNormalLaw_rtP.Saturation_LowerSat_og) { + } else if (rtb_Y_i < LateralNormalLaw_rtP.Saturation_LowerSat_og) { rtb_Y_j = LateralNormalLaw_rtP.Saturation_LowerSat_og; } else { - rtb_Y_j = rtb_Y_p; + rtb_Y_j = rtb_Y_i; } if (LateralNormalLaw_DWork.Delay_DSTATE > LateralNormalLaw_rtP.Limiterxi_UpperSat) { - Vias = LateralNormalLaw_rtP.Limiterxi_UpperSat; + rtb_Y_i = LateralNormalLaw_rtP.Limiterxi_UpperSat; } else if (LateralNormalLaw_DWork.Delay_DSTATE < LateralNormalLaw_rtP.Limiterxi_LowerSat) { - Vias = LateralNormalLaw_rtP.Limiterxi_LowerSat; + rtb_Y_i = LateralNormalLaw_rtP.Limiterxi_LowerSat; } else { - Vias = LateralNormalLaw_DWork.Delay_DSTATE; + rtb_Y_i = LateralNormalLaw_DWork.Delay_DSTATE; } - LateralNormalLaw_RateLimiter(Vias * rtb_Y_j + (LateralNormalLaw_rtP.Constant_Value_l1 - rtb_Y_j) * rtb_Gain_b, + 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_DWork.sf_RateLimiter_j); diff --git a/hdw-a339x/src/wasm/fbw_a320/src/model/LateralNormalLaw.h b/hdw-a339x/src/wasm/fbw_a320/src/model/LateralNormalLaw.h deleted file mode 100644 index 9a9fb3087..000000000 --- a/hdw-a339x/src/wasm/fbw_a320/src/model/LateralNormalLaw.h +++ /dev/null @@ -1,155 +0,0 @@ -#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 deleted file mode 100644 index 62244ab1c..000000000 --- a/hdw-a339x/src/wasm/fbw_a320/src/model/LateralNormalLaw_private.h +++ /dev/null @@ -1,6 +0,0 @@ -#ifndef RTW_HEADER_LateralNormalLaw_private_h_ -#define RTW_HEADER_LateralNormalLaw_private_h_ -#include "rtwtypes.h" -#include "LateralNormalLaw_types.h" -#endif -