From 4be083fcfb234db69b69e74827f75715d7ed1870 Mon Sep 17 00:00:00 2001 From: Joshua Davidson <joshuadavidson2000@gmail.com> Date: Tue, 24 Jul 2018 18:52:36 -0400 Subject: [PATCH] Control: Update FMGC altitude and heading control to V3.2 --- Nasal/FMGC-b.nas | 92 +++-------------------- Systems/fmgc-drivers.xml | 149 +++++++++++++++++++++++++++++--------- Systems/fmgc-pitch.xml | 52 ++++++++++--- Systems/fmgc-roll-yaw.xml | 2 +- 4 files changed, 169 insertions(+), 126 deletions(-) diff --git a/Nasal/FMGC-b.nas b/Nasal/FMGC-b.nas index 0d1c6d5e..ef9ef1e2 100644 --- a/Nasal/FMGC-b.nas +++ b/Nasal/FMGC-b.nas @@ -13,17 +13,14 @@ setprop("/it-autoflight/internal/heading-deg", getprop("/orientation/heading-mag setprop("/it-autoflight/internal/track-deg", getprop("/orientation/track-magnetic-deg")); setprop("/it-autoflight/internal/vert-speed-fpm", 0); setprop("/it-autoflight/internal/heading-error-deg", 0); -setprop("/it-autoflight/internal/heading-5-sec-ahead", 0); -setprop("/it-autoflight/internal/altitude-5-sec-ahead", 0); +setprop("/it-autoflight/internal/heading-predicted", 0); +setprop("/it-autoflight/internal/altitude-predicted", 0); setprop("/it-autoflight/internal/lnav-advance-nm", 1); setlistener("/sim/signals/fdm-initialized", func { var trueSpeedKts = getprop("/instrumentation/airspeed-indicator/true-speed-kt"); var locdefl = getprop("/instrumentation/nav[0]/heading-needle-deflection-norm"); var signal = getprop("/instrumentation/nav[0]/gs-needle-deflection-norm"); - var VS = getprop("/velocities/vertical-speed-fps"); - var TAS = getprop("/velocities/uBody-fps"); - var FPangle = 0; var gear1 = getprop("/gear/gear[1]/wow"); var gear2 = getprop("/gear/gear[2]/wow"); var gnds_mps = 0; @@ -40,6 +37,7 @@ setlistener("/sim/signals/fdm-initialized", func { var R = 0; var dist_coeff = 0; var turn_dist = 0; + var vsnow = 0; }); var APinit = func { @@ -293,7 +291,7 @@ var lateral = func { setprop("/it-autoflight/input/lat-arm", 0); setprop("/it-autoflight/output/loc-armed", 0); setprop("/it-autoflight/output/appr-armed", 0); - var hdg5sec = math.round(getprop("/it-autoflight/internal/heading-5-sec-ahead")); + var hdg5sec = math.round(getprop("/it-autoflight/internal/heading-predicted")); setprop("/it-autoflight/input/hdg", hdg5sec); setprop("/it-autoflight/output/lat", 0); setprop("/it-autoflight/mode/lat", "HDG"); @@ -369,7 +367,7 @@ var vertical = func { } else { setprop("/it-autoflight/mode/arm", " "); } - var alt5sec = math.round(getprop("/it-autoflight/internal/altitude-5-sec-ahead"), 500); + var alt5sec = math.round(getprop("/it-autoflight/internal/altitude-predicted"), 100); setprop("/it-autoflight/input/alt", alt5sec); setprop("/it-autoflight/internal/alt", alt5sec); thrustmode(); @@ -456,11 +454,9 @@ var vertical = func { setprop("/it-autoflight/mode/arm", " "); } } else if (vertset == 5) { - fpa_calc(); alandt.stop(); alandt1.stop(); mng_sys_stop(); - fpa_calct.start(); setprop("/it-autoflight/output/appr-armed", 0); var altinput = getprop("/it-autoflight/input/alt"); setprop("/it-autoflight/internal/alt", altinput); @@ -659,18 +655,6 @@ var alt_on = func { minmaxtimer.start(); } -var fpa_calc = func { - VS = getprop("/velocities/vertical-speed-fps"); - TAS = getprop("/velocities/uBody-fps"); - if (TAS < 10) TAS = 10; - if (VS < -200) VS =-200; - if (abs(VS/TAS) <= 1) { - FPangle = math.asin(VS/TAS); - FPangle *=90; - setprop("/it-autoflight/internal/fpa", FPangle); - } -} - setlistener("/it-autoflight/input/kts-mach", func { var ias = getprop("/it-autoflight/input/spd-kts") or 0; var mach = getprop("/it-autoflight/input/spd-mach") or 0; @@ -746,59 +730,29 @@ var updateTimers = func { var vertm = getprop("/it-autoflight/output/vert"); if (vertm == 1) { altcaptt.start(); - fpa_calct.stop(); } else if (vertm == 4) { altcaptt.start(); - fpa_calct.stop(); } else if (vertm == 5) { altcaptt.start(); } else if (vertm == 7) { altcaptt.start(); - fpa_calct.stop(); } else if (vertm == 8) { altcaptt.stop(); - fpa_calct.stop(); } else if (vertm == 9) { altcaptt.start(); - fpa_calct.stop(); } else { altcaptt.stop(); - fpa_calct.stop(); } } else { altcaptt.start(); - fpa_calct.stop(); } } # Altitude Capture var altcapt = func { - var vsnow = getprop("/it-autoflight/internal/vert-speed-fpm"); - if ((vsnow >= 0 and vsnow < 500) or (vsnow < 0 and vsnow > -500)) { - setprop("/it-autoflight/internal/captvs", 100); - setprop("/it-autoflight/internal/captvsneg", -100); - } else if ((vsnow >= 500 and vsnow < 1000) or (vsnow < -500 and vsnow > -1000)) { - setprop("/it-autoflight/internal/captvs", 200); - setprop("/it-autoflight/internal/captvsneg", -200); - } else if ((vsnow >= 1000 and vsnow < 1500) or (vsnow < -1000 and vsnow > -1500)) { - setprop("/it-autoflight/internal/captvs", 300); - setprop("/it-autoflight/internal/captvsneg", -300); - } else if ((vsnow >= 1500 and vsnow < 2000) or (vsnow < -1500 and vsnow > -2000)) { - setprop("/it-autoflight/internal/captvs", 400); - setprop("/it-autoflight/internal/captvsneg", -400); - } else if ((vsnow >= 2000 and vsnow < 3000) or (vsnow < -2000 and vsnow > -3000)) { - setprop("/it-autoflight/internal/captvs", 600); - setprop("/it-autoflight/internal/captvsneg", -600); - } else if ((vsnow >= 3000 and vsnow < 4000) or (vsnow < -3000 and vsnow > -4000)) { - setprop("/it-autoflight/internal/captvs", 900); - setprop("/it-autoflight/internal/captvsneg", -900); - } else if ((vsnow >= 4000 and vsnow < 5000) or (vsnow < -4000 and vsnow > -5000)) { - setprop("/it-autoflight/internal/captvs", 1200); - setprop("/it-autoflight/internal/captvsneg", -1200); - } else if ((vsnow >= 5000) or (vsnow < -5000)) { - setprop("/it-autoflight/internal/captvs", 1500); - setprop("/it-autoflight/internal/captvsneg", -1500); - } + vsnow = getprop("/it-autoflight/internal/vert-speed-fpm"); + setprop("/it-autoflight/internal/captvs", math.round(vsnow / 5, 50)); + setprop("/it-autoflight/internal/captvsneg", -1 * math.round(vsnow / 5, 50)); if ((getprop("/it-autoflight/output/fd1") == 1 or getprop("/it-autoflight/output/fd2") == 1 or getprop("/it-autoflight/output/ap1") == 1 or getprop("/it-autoflight/output/ap2") == 1) and getprop("/it-autoflight/output/vert") != 9) { var calt = getprop("/instrumentation/altimeter/indicated-altitude-ft"); var alt = getprop("/it-autoflight/internal/alt"); @@ -1216,32 +1170,9 @@ var mng_des_fpm = func { # Managed Capture var mng_altcapt = func { - var vsnow = getprop("/it-autoflight/internal/vert-speed-fpm"); - if ((vsnow >= 0 and vsnow < 500) or (vsnow < 0 and vsnow > -500)) { - setprop("/it-autoflight/internal/captvs", 100); - setprop("/it-autoflight/internal/captvsneg", -100); - } else if ((vsnow >= 500 and vsnow < 1000) or (vsnow < -500 and vsnow > -1000)) { - setprop("/it-autoflight/internal/captvs", 200); - setprop("/it-autoflight/internal/captvsneg", -200); - } else if ((vsnow >= 1000 and vsnow < 1500) or (vsnow < -1000 and vsnow > -1500)) { - setprop("/it-autoflight/internal/captvs", 300); - setprop("/it-autoflight/internal/captvsneg", -300); - } else if ((vsnow >= 1500 and vsnow < 2000) or (vsnow < -1500 and vsnow > -2000)) { - setprop("/it-autoflight/internal/captvs", 400); - setprop("/it-autoflight/internal/captvsneg", -400); - } else if ((vsnow >= 2000 and vsnow < 3000) or (vsnow < -2000 and vsnow > -3000)) { - setprop("/it-autoflight/internal/captvs", 600); - setprop("/it-autoflight/internal/captvsneg", -600); - } else if ((vsnow >= 3000 and vsnow < 4000) or (vsnow < -3000 and vsnow > -4000)) { - setprop("/it-autoflight/internal/captvs", 900); - setprop("/it-autoflight/internal/captvsneg", -900); - } else if ((vsnow >= 4000 and vsnow < 5000) or (vsnow < -4000 and vsnow > -5000)) { - setprop("/it-autoflight/internal/captvs", 1200); - setprop("/it-autoflight/internal/captvsneg", -1200); - } else if ((vsnow >= 5000) or (vsnow < -5000)) { - setprop("/it-autoflight/internal/captvs", 1500); - setprop("/it-autoflight/internal/captvsneg", -1500); - } + vsnow = getprop("/it-autoflight/internal/vert-speed-fpm"); + setprop("/it-autoflight/internal/captvs", math.round(vsnow / 5, 50)); + setprop("/it-autoflight/internal/captvsneg", -1 * math.round(vsnow / 5, 50)); var MNGalt = getprop("/it-autoflight/internal/mng-alt"); var MCPalt = getprop("/it-autoflight/internal/alt"); var MNGdif = abs(MNGalt - MCPalt); @@ -1325,7 +1256,6 @@ var minmaxtimer = maketimer(0.5, minmax); var alandt = maketimer(0.5, aland); var alandt1 = maketimer(0.5, aland1); var reduct = maketimer(0.5, toga_reduc); -var fpa_calct = maketimer(0.1, fpa_calc); var ap_varioust = maketimer(1, ap_various); var mng_maint = maketimer(0.5, mng_main); var mng_altcaptt = maketimer(0.5, mng_altcapt); diff --git a/Systems/fmgc-drivers.xml b/Systems/fmgc-drivers.xml index db4dab87..d6a4ba14 100644 --- a/Systems/fmgc-drivers.xml +++ b/Systems/fmgc-drivers.xml @@ -14,41 +14,6 @@ <!-- Predictors --> <!-- =============================================================== --> - <predict-simple> - <name>HEADING 5 SECONDS AHEAD</name> - <debug>false</debug> - <input> - <condition> - <equals> - <property>/it-autoflight/input/trk</property> - <value>0</value> - </equals> - </condition> - <property>/it-autoflight/internal/heading-deg</property> - </input> - <input> - <condition> - <equals> - <property>/it-autoflight/input/trk</property> - <value>1</value> - </equals> - </condition> - <property>/it-autoflight/internal/track-deg</property> - </input> - <output>/it-autoflight/internal/heading-5-sec-ahead</output> - <seconds>5.0</seconds> - <filter-gain>0.0</filter-gain> - </predict-simple> - - <predict-simple> - <name>ALTITUDE 5 SECONDS AHEAD</name> - <debug>false</debug> - <input>/instrumentation/altimeter/indicated-altitude-ft</input> - <output>/it-autoflight/internal/altitude-5-sec-ahead</output> - <seconds>5.0</seconds> - <filter-gain>0.0</filter-gain> - </predict-simple> - <predict-simple> <name>IAS 5 SECOND PREDICTOR</name> <debug>false</debug> @@ -702,6 +667,120 @@ <max>0</max> </filter> + <filter> + <name>HEADING PREDICTOR STAGE 1</name> + <debug>false</debug> + <type>gain</type> + <gain>1.0</gain> + <input> + <condition> + <equals> + <property>/it-autoflight/input/trk</property> + <value>0</value> + </equals> + </condition> + <expression> + <sum> + <div> + <property>/orientation/roll-deg</property> + <value>1.8</value> + </div> + <property>/it-autoflight/internal/heading-deg</property> + </sum> + </expression> + </input> + <input> + <condition> + <equals> + <property>/it-autoflight/input/trk</property> + <value>1</value> + </equals> + </condition> + <expression> + <sum> + <div> + <property>/orientation/roll-deg</property> + <value>1.8</value> + </div> + <property>/it-autoflight/internal/track-deg</property> + </sum> + </expression> + </input> + <output>/it-autoflight/internal/heading-predicted-cmd</output> + </filter> + + <filter> + <name>HEADING PREDICTOR STAGE 2</name> + <debug>false</debug> + <type>gain</type> + <gain>1.0</gain> + <input> + <condition> + <greater-than> + <property>/it-autoflight/internal/heading-predicted-cmd</property> + <value>360</value> + </greater-than> + </condition> + <expression> + <dif> + <property>/it-autoflight/internal/heading-predicted-cmd</property> + <value>360</value> + </dif> + </expression> + </input> + <input> + <condition> + <less-than> + <property>/it-autoflight/internal/heading-predicted-cmd</property> + <value>1</value> + </less-than> + </condition> + <expression> + <sum> + <property>/it-autoflight/internal/heading-predicted-cmd</property> + <value>360</value> + </sum> + </expression> + </input> + <input>/it-autoflight/internal/heading-predicted-cmd</input> + <output>/it-autoflight/internal/heading-predicted</output> + </filter> + + <filter> + <name>ALTITUDE PREDICTOR</name> + <debug>false</debug> + <type>gain</type> + <gain>1.0</gain> + <input> + <expression> + <sum> + <div> + <property>/it-autoflight/internal/vert-speed-fpm</property> + <value>5</value> + </div> + <property>/instrumentation/altimeter/indicated-altitude-ft</property> + </sum> + </expression> + </input> + <output>/it-autoflight/internal/altitude-predicted</output> + </filter> + + <filter> + <name>INTERNAL FPA COMPUTER</name> + <debug>false</debug> + <type>gain</type> + <gain>1.0</gain> + <input> + <expression> + <dif> + <property>/orientation/pitch-deg</property> + <property>/orientation/alpha-deg</property> + </dif> + </expression> + </input> + <output>/it-autoflight/internal/fpa</output> + </filter> + <!-- =============================================================== --> <!-- Flight Director --> <!-- =============================================================== --> diff --git a/Systems/fmgc-pitch.xml b/Systems/fmgc-pitch.xml index 9548e096..acfbe56b 100644 --- a/Systems/fmgc-pitch.xml +++ b/Systems/fmgc-pitch.xml @@ -18,10 +18,11 @@ <name>ALTITUDE CAPTURE/HOLD</name> <debug>false</debug> <type>gain</type> + <gain>-5</gain> <input> <expression> <sum> - <property>/it-autoflight/internal/altitude-5-sec-ahead</property> + <property>/instrumentation/altimeter/indicated-altitude-ft</property> <value>-2</value> </sum> </expression> @@ -44,7 +45,6 @@ </condition> <property>/it-autoflight/internal/mng-alt</property> </reference> - <gain>-8</gain> <output> <property>/it-autoflight/internal/target-fpm</property> </output> @@ -97,12 +97,20 @@ <property>/it-autoflight/internal/target-fpm-d</property> </output> <config> - <Kp>120</Kp> + <Kp> + <expression> + <table> + <property>/velocities/mach</property> + <entry><ind>0.2</ind><dep>120</dep></entry> + <entry><ind>0.9</ind><dep>340</dep></entry> + </table> + </expression> + </Kp> <beta>1.0</beta> <alpha>0.1</alpha> <gamma>0.0</gamma> - <Ti>1.0</Ti> - <Td>0.00001</Td> + <Ti>2.0</Ti> + <Td>0.002</Td> <u_min> <expression> <dif> @@ -563,7 +571,13 @@ <value>6</value> </not-equals> </condition> - <value>0.0017</value> + <expression> + <table> + <property>/velocities/mach</property> + <entry><ind>0.2</ind><dep>0.0029</dep></entry> + <entry><ind>0.9</ind><dep>0.0010</dep></entry> + </table> + </expression> </Kp> <Kp> <condition> @@ -574,7 +588,7 @@ </condition> <value>0.0055</value> </Kp> - <Ti>3.0</Ti> + <Ti>2.5</Ti> <Td>0.0001</Td> <u_min> <condition> @@ -583,7 +597,17 @@ <value>6</value> </not-equals> </condition> - <value>-10</value> + <expression> + <dif> + <table> + <property>/orientation/pitch-deg</property> + <entry><ind>-10</ind><dep>-5</dep></entry> + <entry><ind> 0</ind><dep> 0</dep></entry> + <entry><ind> 30</ind><dep>35</dep></entry> + </table> + <value>5</value> + </dif> + </expression> </u_min> <u_min> <condition> @@ -607,7 +631,17 @@ </equals> </and> </condition> - <value>30</value> + <expression> + <sum> + <table> + <property>/orientation/pitch-deg</property> + <entry><ind>-10</ind><dep>-15</dep></entry> + <entry><ind> 0</ind><dep> 0</dep></entry> + <entry><ind> 30</ind><dep> 25</dep></entry> + </table> + <value>5</value> + </sum> + </expression> </u_max> <u_max> <condition> diff --git a/Systems/fmgc-roll-yaw.xml b/Systems/fmgc-roll-yaw.xml index 94d47db9..662a5d59 100644 --- a/Systems/fmgc-roll-yaw.xml +++ b/Systems/fmgc-roll-yaw.xml @@ -61,7 +61,7 @@ <property>/it-autoflight/internal/target-roll-deg</property> </output> <config> - <Kp>-1.3</Kp> + <Kp>-1.8</Kp> <Ki>-0.000001</Ki> <min> <property>/it-autoflight/internal/bank-limit</property>