This is my first attempt to reconcile the speed-mode PID process against expectation. That is to say, the PID controller centers on the correct value (process value/PV or reference.)
Here's some context: Set <debug> in the Autopilot.xml file for the "thrust mode: SPD/ias" <pid-controller> module. Line# 456. Departed KBFI northbound, and trimmed for stable flight. Since my altitude was greater-than 300 feet, I set 240 kts in the Autopilot Settings dialog (F11) and clicked the Throttle arm checkbox. The PID seemed to stabilize but near an incorrect target of ~230 Kts.
Previously, I had read the following references on PID structure and behavior and learned that PIDs can be a very extensive topic:
(1) XML Autopilot Config File http://www.flightgear.org/Docs/XMLAutopilot/node4.html
(2) Control Theory 101 http://www.flightgear.org/Docs/XMLAutopilot/node2.html
(3) Wikipedia's PID controller https://en.wikipedia.org/wiki/PID_controller
According to Ref(1), the summation of the products of the respective constants, Kp, Ki, and Kd, and their error terms, should produce an output to replace or adjust the process value. Additionally, Ref(2) tells us that each constant term will have it's own error term, e.g., en = rn - yn. Furthermore, Ref(3) shows that loop tuning can be a difficult process using different methods (depending on one's personal tolerance I guess.)
Here's the last debug excerpt from the console:
Updating Auto Throttle1 ias Ts 0.00833333
input = 229.857 ref = 240
ep_n = 10.1427 ep_n_1 = 10.1442 e_n = 10.1427 ed_n = -229.857 Tf = 1e-05 edf_n = -229.857 delta_u_n = 0.00111142
P:-0.000240962 I:0.00135236 D:2.32035e-08
max saturation
output = 1
Whew, that was a lot. Alright, looking at the excerpt above, I see that the setpoint (SP or reference) equals 240, and that the process value (PV or input) equals 229.857. Calculating the difference gives us -10.1427 (ep_n). So...figuring the error term for Kp is where my expectation went astray. The error term should be the product of Kp and the delta, but there's multiple delta values noted for which I haven't learned the meaning yet (ep_n_1, e_n, ed_n, edf_n). Maybe delta at different time steps? Anyway, go to Line# 480. This is the <config> module for Kp. (Sorry I haven't figured-out how to capture the line numbers in the pasted data of my code editor.)
The contents of the Autopilot.xml file thus:
- Code: Select all
<?xml version="1.0"?>
<PropertyList>
<!-- =============================================================== -->
<!-- Lateral Modes -->
<!-- =============================================================== -->
<pi-simple-controller>
<name>HDG HOLD</name>
<debug>false</debug>
<enable>
<condition>
<equals>
<property>instrumentation/afds/ap-modes/roll-mode</property>
<value>HDG HOLD</value>
</equals>
</condition>
</enable>
<input>
<property>autopilot/internal/fdm-heading-bug-error-deg</property>
</input>
<reference>
<value>0</value>
</reference>
<output>
<property>/autopilot/internal/target-roll-deg</property>
</output>
<config>
<Kp>-1.75</Kp>
<Ki>0.0</Ki>
<u_min>-30</u_min>
<u_max>30</u_max>
<!--u_min>
<property>instrumentation/afds/settings/bank-min</property>
</u_min>
<u_max>
<property>instrumentation/afds/settings/bank-max</property>
</u_max-->
</config>
</pi-simple-controller>
<filter>
<name>LNAV waypoint error computer/normalizer</name>
<debug>false</debug>
<enable>
<property>instrumentation/afds/ap-modes/roll-mode</property>
<value>LNAV</value>
</enable>
<type>gain</type>
<input>
<property>/autopilot/route-manager/wp/bearing-deg</property>
<offset>
<property>/orientation/heading-magnetic-deg</property>
<scale>-1.0</scale>
</offset>
</input>
<output>autopilot/internal/waypoint-bearing-error-deg</output>
<period>
<min>-180</min>
<max>180</max>
</period>
<gain>1.0</gain>
</filter>
<pi-simple-controller>
<name>LNAV1</name>
<debug>false</debug>
<enable>
<property>instrumentation/afds/ap-modes/roll-mode</property>
<value>LNAV</value>
</enable>
<input>
<property>/autopilot/internal/waypoint-bearing-error-deg</property>
</input>
<reference>
<property>/autopilot/internal/course-deviation</property>
<scale>-1.5</scale>
</reference>
<output>
<property>/autopilot/internal/target-roll-deg</property>
</output>
<config>
<Kp>-1.75</Kp>
<Ki>0.0</Ki>
<u_min>-30</u_min>
<u_max>30</u_max>
</config>
</pi-simple-controller>
<pi-simple-controller>
<name>LOC</name>
<debug>false</debug>
<enable>
<property>instrumentation/afds/ap-modes/roll-mode</property>
<value>LOC</value>
</enable>
<input>
<property>/autopilot/internal/nav1-heading-error-deg</property>
</input>
<reference>
<value>0.0</value>
</reference>
<output>
<property>/autopilot/internal/target-roll-deg</property>
</output>
<config>
<Kp>-3.0</Kp>
<Ki>0.0</Ki>
<u_min>-30</u_min>
<u_max>30</u_max>
</config>
</pi-simple-controller>
<pid-controller>
<name>Aileron Control</name>
<debug>false</debug>
<enable>
<property>instrumentation/afds/inputs/AP</property>
</enable>
<input>
<property>/orientation/roll-deg</property>
</input>
<reference>
<property>/autopilot/internal/target-roll-deg</property>
</reference>
<output>
<property>/controls/flight/aileron-filter</property>
</output>
<config>
<Kp>0.007</Kp>
<Ti>10</Ti>
<Td>0.0</Td>
<u_min>
<value>-0.5</value>
</u_min>
<u_max>
<value>0.5</value>
</u_max>
</config>
</pid-controller>
<filter>
<name>Aileron control filter</name>
<debug>false</debug>
<type>noise-spike</type>
<enable>
<property>instrumentation/afds/inputs/AP</property>
</enable>
<max-rate-of-change>0.5</max-rate-of-change>
<initialize-to>output</initialize-to>
<input>
<property>/controls/flight/aileron-filter</property>
</input>
<output>/controls/flight/aileron</output>
</filter>
<!-- =============================================================== -->
<!-- Pitch Axis Modes -->
<!-- =============================================================== -->
<pi-simple-controller>
<name>Altitude Stage 1</name>
<debug>false</debug>
<enable>
<condition>
<or>
<equals>
<property>instrumentation/afds/ap-modes/pitch-mode</property>
<value>ALT</value>
</equals>
<and>
<equals>
<property>instrumentation/afds/ap-modes/pitch-mode</property>
<value>V/S</value>
</equals>
<equals>
<property>autopilot/settings/vertical-speed-fpm</property>
<value>0</value>
</equals>
</and>
</or>
</condition>
</enable>
<input>
<property>instrumentation/altimeter/indicated-altitude-ft</property>
</input>
<reference>
<property>/autopilot/settings/actual-target-altitude-ft</property>
</reference>
<output>
<property>/autopilot/internal/target-climb-rate-fps</property>
</output>
<config>
<Kp>0.15</Kp>
<Ki>0.01</Ki>
<u_min>
<value>-20.00</value>
</u_min>
<u_max>
<value>20.00</value>
</u_max>
</config>
</pi-simple-controller>
<pid-controller>
<name>Altitude Hold (Altimeter based) Stage 2</name>
<debug>false</debug>
<enable>
<condition>
<or>
<equals>
<property>instrumentation/afds/ap-modes/pitch-mode</property>
<value>ALT</value>
</equals>
<and>
<equals>
<property>instrumentation/afds/ap-modes/pitch-mode</property>
<value>V/S</value>
</equals>
<equals>
<property>autopilot/settings/vertical-speed-fpm</property>
<value>0</value>
</equals>
</and>
</or>
</condition>
</enable>
<input>
<property>/velocities/vertical-speed-fps</property>
</input>
<reference>
<property>/autopilot/internal/target-climb-rate-fps</property>
</reference>
<output>
<property>/autopilot/internal/target-pitch-deg</property>
</output>
<config>
<Kp>
<condition>
<greater-than>
<property>instrumentation/altimeter/indicated-altitude-ft</property>
<value>26000</value>
</greater-than>
</condition>
<value>0.1</value>
</Kp>
<Kp>
<condition>
<greater-than>
<property>instrumentation/altimeter/indicated-altitude-ft</property>
<value>14000</value>
</greater-than>
</condition>
<value>0.2</value>
</Kp>
<Kp>0.5</Kp>
<beta>1.0</beta>
<alpha>0.1</alpha>
<gamma>0.0</gamma>
<Ti>10.0</Ti>
<Td>0.005</Td>
<u_min>-15</u_min>
<u_max>15</u_max>
<!--u_min>
<property>instrumentation/afds/settings/pitch-min</property>
</u_min>
<u_max>
<property>instrumentation/afds/settings/pitch-max</property>
</u_max-->
</config>
</pid-controller>
<pid-controller>
<name>Glideslope Hold</name>
<debug>false</debug>
<enable>
<property>instrumentation/afds/ap-modes/pitch-mode</property>
<value>G/S</value>
</enable>
<input>
<property>/velocities/vertical-speed-fps</property>
</input>
<reference>
<property>/instrumentation/nav[0]/gs-rate-of-climb</property>
</reference>
<output>
<property>/autopilot/internal/target-pitch-deg</property>
</output>
<config>
<Kp>0.45</Kp>
<Td>0.0007</Td>
<Ti>50.0</Ti>
<u_min>-15</u_min>
<u_max>15</u_max>
</config>
</pid-controller>
<pid-controller>
<name>VS</name>
<debug>false</debug>
<enable>
<condition>
<and>
<equals>
<property>instrumentation/afds/ap-modes/pitch-mode</property>
<value>V/S</value>
</equals>
<not>
<equals>
<property>autopilot/settings/vertical-speed-fpm</property>
<value>0</value>
</equals>
</not>
</and>
</condition>
</enable>
<input>
<property>/velocities/vertical-speed-fps</property>
</input>
<reference>
<property>/autopilot/settings/vertical-speed-fpm</property>
<scale>0.01666666</scale>
</reference>
<output>
<property>/autopilot/internal/target-pitch-deg</property>
</output>
<config>
<Ts>0.05</Ts>
<Kp>0.15</Kp>
<beta>1.0</beta>
<alpha>0.1</alpha>
<gamma>0.0</gamma>
<Ti>10.0</Ti>
<Td>0.00001</Td>
<u_min>-15</u_min>
<u_max>15</u_max>
<!--u_min>
<property>instrumentation/afds/settings/pitch-min</property>
</u_min>
<u_max>
<property>instrumentation/afds/settings/pitch-max</property>
</u_max-->
</config>
</pid-controller>
<pid-controller>
<name>Elevator Control</name>
<debug>false</debug>
<enable>
<property>instrumentation/afds/inputs/AP</property>
</enable>
<input>
<property>/orientation/pitch-deg</property>
</input>
<reference>
<property>/autopilot/internal/target-pitch-deg</property>
</reference>
<output>
<property>/controls/flight/elevator-trim-filter</property>
</output>
<config>
<Kp>-0.020</Kp>
<Ti>10.0</Ti>
<Td>0.0</Td>
<u_min>
<value>-0.8</value>
</u_min>
<u_max>
<value>0.8</value>
</u_max>
</config>
</pid-controller>
<filter>
<name>Elevator control filter</name>
<debug>false</debug>
<type>noise-spike</type>
<enable>
<property>instrumentation/afds/inputs/AP</property>
</enable>
<max-rate-of-change>0.2</max-rate-of-change>
<initialize-to>output</initialize-to>
<input>
<property>/controls/flight/elevator-trim-filter</property>
</input>
<output>/controls/flight/elevator-trim</output>
</filter>
<filter>
<name>IAS speed limit</name>
<type>gain</type>
<gain>1.0</gain>
<input>
<condition>
<greater-than>
<property>/controls/flight/flaps</property>
<value>0.0</value>
</greater-than>
</condition>
<expression>
<table>
<property>/controls/flight/flaps</property>
<entry><ind>0.033</ind><dep>255</dep></entry>
<entry><ind>0.166</ind><dep>235</dep></entry>
<entry><ind>0.5</ind><dep>215</dep></entry>
<entry><ind>0.666</ind><dep>200</dep></entry>
<entry><ind>0.833</ind><dep>190</dep></entry>
<entry><ind>1.0</ind><dep>180</dep></entry>
</table>
</expression>
</input>
<input>
<condition>
<greater-than>
<property>/gear/gear/position-norm</property>
<value>0.0</value>
</greater-than>
</condition>
<value>270</value>
</input>
<input>
<!-- basic IAS limit based upon diagram 7771003, Sec1.Page4 in the
continental manual -->
<expression>
<table>
<property>instrumentation/altimeter/indicated-altitude-ft</property>
<entry><ind>0</ind><dep>330</dep></entry> <!-- clean Vmo -->
<entry><ind>30000</ind><dep>330</dep></entry>
<entry><ind>43000</ind><dep>248</dep></entry>
</table>
</expression>
</input>
<output>instrumentation/afds/max-airspeed-kts</output>
</filter>
<filter>
<type>gain</type>
<gain>1.0</gain>
<input>
<expression>
<min>
<property>/autopilot/settings/target-speed-kt</property>
<property>instrumentation/afds/max-airspeed-kts</property>
</min>
</expression>
</input>
<output>instrumentation/afds/ias-target-kt</output>
</filter>
<!-- thrust mode: SPD/ias -->
<pid-controller>
<name>Auto Throttle1 ias</name>
<debug>true</debug>
<enable>
<condition>
<property>/instrumentation/afds/inputs/at-armed</property> # original "at-armed[0]"
<equals>
<property>/instrumentation/afds/inputs/autothrottle-index</property>
<value>1</value>
</equals>
<honor-passive>true</honor-passive>
</condition>
</enable>
<initialize-to>output</initialize-to>
<input>
<prop>/instrumentation/airspeed-indicator/indicated-speed-kt</prop>
</input>
<reference>
<prop>instrumentation/afds/ias-target-kt</prop>
</reference>
<output>
<prop>/controls/engines/engine[0]/throttle</prop>
</output>
<config>
<Kp>
<condition>
<and>
<equals>
<property>instrumentation/afds/ap-modes/pitch-mode</property>
<value>G/S</value>
</equals>
<less-than>
<property>position/altitude-agl-ft</property>
<value>200</value>
</less-than>
</and>
</condition>
<value>0.0001</value>
</Kp>
<Kp>
<condition>
<greater-than>
<property>/instrumentation/airspeed-indicator/indicated-speed-kt</property>
<property>instrumentation/afds/ias-target-kt</property>
</greater-than>
</condition>
<value>0.08</value>
</Kp>
<Kp>0.16</Kp>
<Td>0.0001</Td>
<Ti>
<condition>
<greater-than>
<property>/instrumentation/airspeed-indicator/indicated-speed-kt</property>
<property>instrumentation/afds/ias-target-kt</property>
</greater-than>
</condition>
<value>20</value>
</Ti>
<Ti>10</Ti>
<u_min>
<condition>
<equals>
<property>instrumentation/afds/ap-modes/pitch-mode</property>
<value>G/S</value>
</equals>
</condition>
<value>0.06</value>
</u_min>
<u_min>0.125</u_min>
<u_max>1</u_max>
</config>
</pid-controller>
</PropertyList>
The first <condition> tag shows that if "pitch-mode" = G/S AND "altitude-agl-ft < 200 then Kp = 0.0001. I found this <condition> false.
The second <condition> tag shows that if "indicated-speed-kt" and "ias-target-kt" greater-than (what?) then Kp = 0.08. I think this portion is missing an element.
If these two condition tags render false then Kp = 0.16 right?
So, the Kp error term would be: 0.16 x ( 229.857 - 240 ) = -1.623 right? But the Kp error term per the excerpt is P:-0.000240962 if I'm interpreting correctly. If I work the problem backwards I have: un = Kp x ep_n or Kp = un / ep_n, thus Kp = -0.000240962 / -10.1427 which equals 0.0000237572 or 2.37x10exp-5. This suggests that Kp is otherwise assigned some value other than the <condition> tags or "else" branches identify. Again, if my interpretation of the console is correct.
Finally, the <u_min> and <u_max> tags clamp the output. The sum of the P, I, D values from the console equals 0.001111137, which seems below the clamp values and so shouldn't the minimum (0.125) throttle percentage be applied? But it's not, the console excerpt states that "max saturation" is established (?) and that the final output equals 1. Additionally, my frame rate plummets from ~30 to 1 with corresponding frame-rate-spacing ~1400 ms when I click the "Throttle arm" checkbox; although my weather and graphics settings might have a lot to do with this.
I can watch the HUD throttle position symbol move up and down the scale as the PID controller adjusts, but I'm not certain that the tuning error(s) are in the .xml code, within the AFDS.nas code, or both. I'm betting on the .xml code
Could I have your thoughts please?
Thanks,
Eric
Here's the AFDS.nas code just in case:
- Code: Select all
#############################################################################
# PC-9M Autopilot Flight Director System
# Hyde Yamakawa
#
# speed modes: SPD;
# roll modes : HDG SEL,HDG HOLD, LNAV, LOC;
# pitch modes: ALT, V/S, G/S;
# FPA range : -9.9 ~ 9.9 degrees
# VS range : -3880 ~ 3880 original -8000 to 6000
# ALT range : 0 ~ 37,992 original 50,000
# KIAS range : 69 ~ 320 original 100 to 399
# MACH range : 0.40 ~ 0.95
#
#############################################################################
#Usage : var afds = AFDS.new();
var copilot = func(msg) { setprop("/sim/messages/copilot",msg);}
var AFDS =
{
new : func
{
var m = {parents:[AFDS]};
m.spd_list=["","SPD"];
m.roll_list=["","HDG HOLD","LNAV","LOC"];
m.pitch_list=["","ALT","V/S","G/S"];
m.step=0;
m.remaining_distance_log_last = 36000;
m.heading_change_rate = 0.4 * 0.7;
m.AFDS_node = props.globals.getNode("instrumentation/afds",1);
m.AFDS_inputs = m.AFDS_node.getNode("inputs",1);
m.AFDS_apmodes = m.AFDS_node.getNode("ap-modes",1);
m.AFDS_settings = m.AFDS_node.getNode("settings",1);
m.AP_settings = props.globals.getNode("autopilot/settings",1);
m.AP = m.AFDS_inputs.initNode("AP",0,"BOOL");
m.AP_disengaged = m.AFDS_inputs.initNode("AP-disengage",0,"BOOL");
m.AP_passive = props.globals.initNode("autopilot/locks/passive-mode",1,"BOOL"); # original 1
m.AP_pitch_engaged = props.globals.initNode("autopilot/locks/pitch-engaged",0,"BOOL"); # original 1
m.AP_roll_engaged = props.globals.initNode("autopilot/locks/roll-engaged",0,"BOOL"); # original 1
m.FD = m.AFDS_inputs.initNode("FD",0,"BOOL");
m.at1 = m.AFDS_inputs.initNode("at-armed",0,"BOOL");
m.autothrottle_mode = m.AFDS_inputs.initNode("autothrottle-index",0,"INT");
m.lateral_mode = m.AFDS_inputs.initNode("lateral-index",0,"INT");
m.vertical_mode = m.AFDS_inputs.initNode("vertical-index",0,"INT");
m.gs_armed = m.AFDS_inputs.initNode("gs-armed",0,"BOOL");
m.loc_armed = m.AFDS_inputs.initNode("loc-armed",0,"BOOL");
m.vor_armed = m.AFDS_inputs.initNode("vor-armed",0,"BOOL");
m.lnav_armed = m.AFDS_inputs.initNode("lnav-armed",0,"BOOL");
m.estimated_time_arrival = m.AFDS_inputs.initNode("estimated-time-arrival",0,"INT");
m.remaining_distance = m.AFDS_inputs.initNode("remaining-distance",0,"DOUBLE");;
m.vs_ind = props.globals.initNode("autopilot/internal/vert-speed-fpm",0,"DOUBLE");
m.heading_magnetic = m.AFDS_settings.getNode("heading-magnetic",1);
m.AP_roll_mode = m.AFDS_apmodes.initNode("roll-mode","");
m.AP_pitch_mode = m.AFDS_apmodes.initNode("pitch-mode","");
m.AP_speed_mode = m.AFDS_apmodes.initNode("speed-mode","");
m.ias_setting = m.AP_settings.initNode("target-speed-kt",200);# 100 - 399 #
m.vs_setting = m.AP_settings.initNode("vertical-speed-fpm",0); # -3880 to +3880 #
m.hdg_setting = m.AP_settings.initNode("heading-bug-deg",360,"INT"); # 1 to 360
m.alt_setting = m.AP_settings.initNode("counter-set-altitude-ft",1200,"DOUBLE");
m.target_alt = m.AP_settings.initNode("actual-target-altitude-ft",2000,"DOUBLE");
m.APl = setlistener(m.AP, func m.setAP(),0,0);
m.APdisl = setlistener(m.AP_disengaged, func m.setAP(),0,0);
return m;
},
#### Inputs ####
###################
input : func(mode,btn)
{
# if(getprop("/systems/electrical/outputs/autopilot")) # changed to "autopilot" 11-17-18 because "avionics" specific property did not exist in the property tree
# {
var current_alt = getprop("instrumentation/altimeter/indicated-altitude-ft");
if(mode==0)
{
# horizontal AP controls
if(btn == 1) # Heading button
{
# set target to current magnetic heading
var tgtHdg = int(me.heading_magnetic.getValue() + 0.50);
me.hdg_setting.setValue(tgtHdg);
}
elsif(btn==2) # LNAV button
{
if ((!getprop("/autopilot/route-manager/active"))or
(getprop("/autopilot/route-manager/current-wp")<0)or
(getprop("/autopilot/route-manager/wp/id")==""))
{
# Oops, route manager isn't active. Keep current mode.
btn = me.lateral_mode.getValue();
copilot("Instructor:LNAV doesn't engage. We forgot to program or activate the route manager!");
}
else
{
if(me.lateral_mode.getValue() == 2) # Current mode is LNAV
{
# set target to current magnetic heading
var tgtHdg = int(me.heading_magnetic.getValue() + 0.50);
me.hdg_setting.setValue(tgtHdg);
btn = 1; # Heading sel
}
elsif(me.lnav_armed.getValue())
{ # LNAV armed then disarm
me.lnav_armed.setValue(0);
btn = me.lateral_mode.getValue();
}
else
{ # LNAV arm
me.lnav_armed.setValue(1);
btn = me.lateral_mode.getValue();
}
}
}
me.lateral_mode.setValue(btn);
}
elsif(mode==1)
{
# vertical AP controls
if (btn==1)
{
# hold current altitude
if(me.AP.getValue() or me.FD.getValue())
{
var alt = int((current_alt+50)/100)*100;
me.target_alt.setValue(alt);
# me.autothrottle_mode.setValue(5); # A/T SPD
}
}
if(btn==2)
{
# hold current vertical speed
var vs = me.vs_ind.getValue();
vs = int(vs/100)*100;
if (vs<-3880) vs = -3880;
if (vs>3880) vs = 3880;
me.vs_setting.setValue(vs);
me.target_alt.setValue(me.alt_setting.getValue());
# me.autothrottle_mode.setValue(5); # A/T SPD
}
me.vertical_mode.setValue(btn);
}
elsif(mode == 2)
{
# throttle AP controls
if(me.autothrottle_mode.getValue() != 0
or (me.at1.getValue() == 0))
{
btn = 0;
}
elsif(getprop("position/altitude-agl-ft") < 400) # below baro 400 ft. changed to < on 11-18-18
{
btn=0;
copilot("Auto-throttle won't engage below 400ft.");
}
me.autothrottle_mode.setValue(btn);
}
elsif(mode==3) #FD, LOC or G/S button
{
var llocmode = me.lateral_mode.getValue();
if(btn==0)
{
if(llocmode == 3) # Alrady in LOC mode
{
# set target to current magnetic heading
var tgtHdg = int(me.heading_magnetic.getValue() + 0.50);
me.hdg_setting.setValue(tgtHdg);
me.lateral_mode.setValue(2); # Keep current headding
me.loc_armed.setValue(0); # Disarm
}
elsif(me.loc_armed.getValue()) # LOC armed but not captured yet
{
me.loc_armed.setValue(0); # Disarm
}
else
{
me.loc_armed.setValue(1); # LOC arm
}
}
elsif (btn==1) #APP button
{
var lgsmode = me.vertical_mode.getValue();
if(lgsmode == 3) # Already in G/S mode
{
me.vertical_mode.setValue(1); # Keep current altitude
me.gs_armed.setValue(0); # Disarm
}
elsif(me.gs_armed.getValue()) # G/S armed but not captured yet
{
me.gs_armed.setValue(0); # Disarm
if(llocmode == 3) # Alrady in LOC mode
{
# set target to current magnetic heading
var tgtHdg = int(me.heading_magnetic.getValue() + 0.50);
me.hdg_setting.setValue(tgtHdg);
me.lateral_mode.setValue(1); # Keep current headding
me.loc_armed.setValue(0); # Disarm
}
else
{
me.loc_armed.setValue(0); # Disarm
}
}
else
{
me.gs_armed.setValue(1); # G/S arm
if(me.loc_armed.getValue() == 0)
{
me.loc_armed.setValue(1); # LOC arm
}
}
}
elsif(btn == 2) # FD button toggle
{
if(!me.FD.getValue())
{
if(me.lateral_mode.getValue() == 0) # Not set
{
# set target to current magnetic heading
var tgtHdg = int(me.heading_magnetic.getValue() + 0.50);
me.hdg_setting.setValue(tgtHdg);
me.lateral_mode.setValue(1); # HDG HOLD
}
if(me.vertical_mode.getValue() == 0) # Not set
{
var alt = int((current_alt+50)/100)*100;
me.target_alt.setValue(alt);
me.vertical_mode.setValue(1); # ALT
}
me.FD.setValue(1)
}
else
{
if(!me.AP.getValue())
{
me.lateral_mode.setValue(0); # Clear
me.vertical_mode.setValue(0); # Clear
}
me.FD.setValue(0)
}
}
}
# }
},
###################
setAP : func{
var output = 1-me.AP.getValue();
var disabled = me.AP_disengaged.getValue();
if((output==0)and(getprop("position/altitude-agl-ft")<200))
{
disabled = 1;
copilot("Instructor:Autopilot won't engage below 200ft.");
}
if((disabled)and(output==0)){output = 1;me.AP.setValue(0);}
if (output==1)
{
var msg="";
var msg2="";
var msg3="";
if (abs(getprop("controls/flight/rudder-trim")) > 0.04) msg = "rudder";
if (abs(getprop("controls/flight/elevator-trim")) > 0.04) msg2 = "pitch";
if (abs(getprop("controls/flight/aileron-trim")) > 0.04) msg3 = "aileron";
if (msg ~ msg2 ~ msg3 != "")
{
if ((msg != "")and(msg2!=""))
msg = msg ~ ", " ~ msg2;
else
msg = msg ~ msg2;
if ((msg != "")and(msg3!=""))
msg = msg ~ " and " ~ msg3;
else
msg = msg ~ msg3;
copilot("Instructor:Autopilot disengaged. Careful, check " ~ msg ~ " trim!");
}
me.loc_armed.setValue(0); # Disarm
me.gs_armed.setValue(0); # Disarm
if(!me.FD.getValue())
{
me.lateral_mode.setValue(0); # NO MODE
me.vertical_mode.setValue(0); # NO MODE
}
else
{
me.lateral_mode.setValue(1); # HDG HOLD
me.vertical_mode.setValue(1); # ALT
}
me.autothrottle_mode.setValue(0);
}
else
{
if(me.lateral_mode.getValue() == 0) # Not set
{
me.input(0,1);
}
if(me.vertical_mode.getValue() == 0) # Not set
{
me.input(1,1);
}
}
},
#################
ap_update : func
{
var current_alt = getprop("instrumentation/altimeter/indicated-altitude-ft");
var VS = getprop("velocities/vertical-speed-fps");
var TAS = getprop("velocities/uBody-fps");
if(me.step == 0)
{ ### glideslope armed ?###
if(me.gs_armed.getValue())
{
var gsdefl = getprop("instrumentation/nav/gs-needle-deflection");
var gsrange = getprop("instrumentation/nav/gs-in-range");
if ((gsdefl< 0.5 and gsdefl>-0.5)and
gsrange)
{
me.vertical_mode.setValue(3);
me.gs_armed.setValue(0);
}
}
}
elsif(me.step == 1)
{ ### localizer armed ? ###
if(me.loc_armed.getValue())
{
if (getprop("instrumentation/nav/in-range"))
{
if(!getprop("instrumentation/nav/nav-loc"))
{
var vheading = getprop("instrumentation/nav/radials/selected-deg");
var vvor = getprop("instrumentation/nav/heading-deg");
var vdist = getprop("instrumentation/nav/nav-distance");
var vorient = getprop("environment/magnetic-variation-deg");
var vmag = getprop("orientation/heading-magnetic-deg");
var vspeed = getprop("/instrumentation/airspeed-indicator/indicated-mach");
var deg_to_rad = math.pi / 180;
var vdiff = abs(vheading - vvor + vorient);
vdiff = abs(vdist * math.sin(vdiff * deg_to_rad));
var vlim = vspeed / 0.3 * 1300 * abs(vheading - vmag) / 45 ;
if(vdiff < vlim)
{
me.lateral_mode.setValue(3);
me.loc_armed.setValue(0);
}
}
else
{
var hddefl = getprop("instrumentation/nav/heading-needle-deflection");
if(abs(hddefl) < 9.9)
{
me.lateral_mode.setValue(3);
me.loc_armed.setValue(0);
var vradials = getprop("instrumentation/nav[0]/radials/target-radial-deg")
- getprop("environment/magnetic-variation-deg") + 0.5;
if(vradials < 0.5) vradials += 360;
elsif(vradials >= 360.5) vradials -= 360;
me.hdg_setting.setValue(vradials);
}
}
}
}
elsif(me.lnav_armed.getValue())
{
if(getprop("position/altitude-agl-ft") > 50)
{
me.lnav_armed.setValue(0); # Clear
me.lateral_mode.setValue(2); # LNAV
}
}
}
elsif(me.step == 2)
{ ### check lateral modes ###
var vheading = getprop("orientation/heading-magnetic-deg");
if(vheading < 0.5)
{
vheading += 360;
}
me.heading_magnetic.setValue(vheading);
var idx = me.lateral_mode.getValue();
me.AP_roll_mode.setValue(me.roll_list[idx]);
me.AP_roll_engaged.setBoolValue(idx > 0);
}
elsif(me.step == 3)
{ ### check vertical modes ###
var idx = me.vertical_mode.getValue();
var offset = (abs(me.vs_ind.getValue()) / 8);
if(offset < 20)
{
offset = 20;
}
if((idx==1)or(idx==2))
{
if (abs(current_alt - me.alt_setting.getValue()) < offset)
{
# within MCP altitude: switch to ALT HOLD mode
idx = 1; # ALT
# if(me.autothrottle_mode.getValue() != 0)
# {
# me.autothrottle_mode.setValue(5); # A/T SPD
# }
me.vs_setting.setValue(0);
}
}
me.vertical_mode.setValue(idx);
me.AP_pitch_mode.setValue(me.pitch_list[idx]);
}
elsif(me.step == 4) ### Auto Throttle mode control ###
{
# Auto throttle arm switch is offed
if(me.at1.getValue() == 0)
{
me.autothrottle_mode.setValue(0);
}
# AP and A/T off when below AGL100ft
elsif(getprop("position/altitude-agl-ft") < 100)
{
me.autothrottle_mode.setValue(0);
me.AP.setValue(0);
}
# Take off mode and above baro 400 ft
elsif(getprop("position/altitude-agl-ft") > 400)
{
if(me.at1.getValue() == 1)
{
me.autothrottle_mode.setValue(1);
}
}
idx = me.autothrottle_mode.getValue();
me.AP_speed_mode.setValue(me.spd_list[idx]);
}
elsif(me.step == 5)
{
if (getprop("/autopilot/route-manager/active")){
var max_wpt = getprop("/autopilot/route-manager/route/num");
var atm_wpt = getprop("/autopilot/route-manager/current-wp");
var destination_elevation = getprop("/autopilot/route-manager/destination/field-elevation-ft");
var total_distance = getprop("/autopilot/route-manager/total-distance");
if(me.lateral_mode.getValue() == 2) # Current mode is LNAV
{
if(atm_wpt < (max_wpt - 1))
{
me.remaining_distance.setValue(getprop("/autopilot/route-manager/wp/remaining-distance-nm")
+ getprop("autopilot/route-manager/wp/dist"));
var next_course = getprop("/autopilot/route-manager/wp[1]/bearing-deg");
}
else
{
me.remaining_distance.setValue(getprop("autopilot/route-manager/wp/dist"));
}
}
if(getprop("/autopilot/route-manager/active"))
{
var wpt_distance = getprop("autopilot/route-manager/wp/dist");
var groundspeed = getprop("/velocities/groundspeed-kt");
if(wpt_distance != nil)
{
var wpt_eta = (wpt_distance / groundspeed * 3600);
var gmt = getprop("instrumentation/clock/indicated-sec");
if((getprop("gear/gear[1]/wow") == 0) and (getprop("gear/gear[2]/wow") == 0))
{
gmt += (wpt_eta + 30);
var gmt_hour = int(gmt / 3600);
if(gmt_hour > 24)
{
gmt_hour -= 24;
gmt -= 24 * 3600;
}
me.estimated_time_arrival.setValue(gmt_hour * 100 + int((gmt - gmt_hour * 3600) / 60));
var change_wp = abs(getprop("/autopilot/route-manager/wp[1]/bearing-deg") - me.heading_magnetic.getValue());
if(change_wp > 180) change_wp = (360 - change_wp);
if(((me.heading_change_rate * change_wp) > wpt_eta)
or (wpt_distance < 0.4)
or ((me.remaining_distance_log_last < wpt_distance) and (change_wp < 80)))
{
if(atm_wpt < (max_wpt - 1))
{
atm_wpt += 1;
props.globals.getNode("/autopilot/route-manager/current-wp").setValue(atm_wpt);
me.altitude_restriction = getprop("/autopilot/route-manager/route/wp["~atm_wpt~"]/altitude-ft");
}
me.remaining_distance_log_last = 36000;
}
else
{
me.remaining_distance_log_last = wpt_distance;
}
}
if(getprop("/autopilot/internal/waypoint-bearing-error-deg") != nil)
{
if(abs(getprop("/position/latitude-deg")) < 80)
{
if(abs(getprop("/instrumentation/gps/wp/wp[1]/course-error-nm")) < 2)
{
setprop("/autopilot/internal/course-deviation", getprop("/instrumentation/gps/wp/wp[1]/course-error-nm"))
}
elsif(getprop("/instrumentation/gps/wp/wp[1]/course-deviation-deg") < 2)
{
setprop("/autopilot/internal/course-deviation", getprop("/instrumentation/gps/wp/wp[1]/course-deviation-deg"))
}
else
{
setprop("/autopilot/internal/course-deviation", 0);
}
}
else
{
setprop("/autopilot/internal/course-deviation", 0);
}
}
}
}
}
}
me.step+=1;
if(me.step > 5) me.step = 0;
},
};
#####################
var click_reset = func(propName) {
setprop(propName,0);
}
controls.click = func(button) {
if (getprop("sim/freeze/replay-state"))
return;
var propName="sim/sound/click"~button;
setprop(propName,1);
settimer(func { click_reset(propName) },0.4);
}
var afds = AFDS.new();
var afds_init_listener = setlistener("/sim/signals/fdm-initialized", func {
removelistener(afds_init_listener);
settimer(update_afds,6);
print("AFDS System ... OK");
});
var update_afds = func {
afds.ap_update();
settimer(update_afds, 0);
}