777: add better check for capturing a localizer
[fg:toms-fgdata.git] / Aircraft / 777-200 / Nasal / AFDS.nas
1 #############################################################################
2 # 777 Autopilot Flight Director System
3 # Syd Adams
4 #
5 # speed modes: THR,THR REF, IDLE,HOLD,SPD;
6 # roll modes : TO/GA,HDG SEL,HDG HOLD, LNAV,LOC,ROLLOUT,TRK SEL, TRK HOLD,ATT;
7 # pitch modes: TO/GA,ALT,V/S,VNAV PTH,VNAV SPD,VNAV ALT,G/S,FLARE,FLCH SPD,FPA;
8 # FPA range  : -9.9 ~ 9.9 degrees
9 # VS range   : -8000 ~ 6000
10 # ALT range  : 0 ~ 50,000
11 # KIAS range : 100 ~ 399
12 # MACH range : 0.40 ~ 0.95
13 #
14 #############################################################################
15
16 #Usage : var afds = AFDS.new();
17
18 var copilot = func(msg) { setprop("/sim/messages/copilot",msg);}
19
20 var AFDS = {
21     new : func{
22         var m = {parents:[AFDS]};
23
24         m.spd_list=["","THR","THR REF","HOLD","IDLE","SPD"];
25
26         m.roll_list=["","HDG SEL","HDG HOLD","LNAV","LOC","ROLLOUT",
27         "TRK SEL","TRK HOLD","ATT","TO/GA"];
28
29         m.pitch_list=["","ALT","V/S","VNAV PTH","VNAV SPD",
30         "VNAV ALT","G/S","FLARE","FLCH SPD","FPA","TO/GA"];
31
32         m.step=0;
33
34         m.AFDS_node = props.globals.getNode("instrumentation/afds",1);
35         m.AFDS_inputs = m.AFDS_node.getNode("inputs",1);
36         m.AFDS_apmodes = m.AFDS_node.getNode("ap-modes",1);
37         m.AFDS_settings = m.AFDS_node.getNode("settings",1);
38         m.AP_settings = props.globals.getNode("autopilot/settings",1);
39
40         m.AP = m.AFDS_inputs.initNode("AP",0,"BOOL");
41         m.AP_disengaged = m.AFDS_inputs.initNode("AP-disengage",0,"BOOL");
42         m.AP_passive = props.globals.initNode("autopilot/locks/passive-mode",1,"BOOL");
43         m.AP_pitch_engaged = props.globals.initNode("autopilot/locks/pitch-engaged",1,"BOOL");
44         m.AP_roll_engaged = props.globals.initNode("autopilot/locks/roll-engaged",1,"BOOL");
45
46         m.FD = m.AFDS_inputs.initNode("FD",0,"BOOL");
47         m.at1 = m.AFDS_inputs.initNode("at-armed[0]",0,"BOOL");
48         m.at2 = m.AFDS_inputs.initNode("at-armed[1]",0,"BOOL");
49         m.alt_knob = m.AFDS_inputs.initNode("alt-knob",0,"BOOL");
50         m.autothrottle_mode = m.AFDS_inputs.initNode("autothrottle-index",0,"INT");
51         m.lateral_mode = m.AFDS_inputs.initNode("lateral-index",0,"INT");
52         m.vertical_mode = m.AFDS_inputs.initNode("vertical-index",0,"INT");
53         m.gs_armed = m.AFDS_inputs.initNode("gs-armed",0,"BOOL");
54         m.loc_armed = m.AFDS_inputs.initNode("loc-armed",0,"BOOL");
55         m.vor_armed = m.AFDS_inputs.initNode("vor-armed",0,"BOOL");
56         m.ias_mach_selected = m.AFDS_inputs.initNode("ias-mach-selected",0,"BOOL");
57         m.hdg_trk_selected = m.AFDS_inputs.initNode("hdg-trk-selected",0,"BOOL");
58         m.vs_fpa_selected = m.AFDS_inputs.initNode("vs-fpa-selected",0,"BOOL");
59         m.bank_switch = m.AFDS_inputs.initNode("bank-limit-switch",0,"INT");
60
61         m.ias_setting = m.AP_settings.initNode("target-speed-kt",200);# 100 - 399 #
62         m.mach_setting = m.AP_settings.initNode("target-speed-mach",0.40);# 0.40 - 0.95 #
63         m.vs_setting = m.AP_settings.initNode("vertical-speed-fpm",0); # -8000 to +6000 #
64         m.hdg_setting = m.AP_settings.initNode("heading-bug-deg",360,"INT"); # 1 to 360
65         m.fpa_setting = m.AP_settings.initNode("flight-path-angle",0); # -9.9 to 9.9 #
66         m.alt_setting = m.AP_settings.initNode("target-altitude-ft",10000,"DOUBLE");
67         m.auto_brake_setting = m.AP_settings.initNode("autobrake",0.000,"DOUBLE");
68
69         m.trk_setting = m.AFDS_settings.initNode("trk",0,"INT");
70         m.vs_display = m.AFDS_settings.initNode("vs-display",0);
71         m.fpa_display = m.AFDS_settings.initNode("fpa-display",0);
72         m.bank_min = m.AFDS_settings.initNode("bank-min",-25);
73         m.bank_max = m.AFDS_settings.initNode("bank-max",25);
74         m.pitch_min = m.AFDS_settings.initNode("pitch-min",-10);
75         m.pitch_max = m.AFDS_settings.initNode("pitch-max",15);
76         m.vnav_alt = m.AFDS_settings.initNode("vnav-alt",35000);
77
78         m.AP_roll_mode = m.AFDS_apmodes.initNode("roll-mode","TO/GA");
79         m.AP_roll_arm = m.AFDS_apmodes.initNode("roll-mode-arm"," ");
80         m.AP_pitch_mode = m.AFDS_apmodes.initNode("pitch-mode","TO/GA");
81         m.AP_pitch_arm = m.AFDS_apmodes.initNode("pitch-mode-arm"," ");
82         m.AP_speed_mode = m.AFDS_apmodes.initNode("speed-mode","");
83         m.AP_annun = m.AFDS_apmodes.initNode("mode-annunciator"," ");
84
85         m.APl = setlistener(m.AP, func m.setAP(),0,0);
86         m.APdisl = setlistener(m.AP_disengaged, func m.setAP(),0,0);
87         m.Lbank = setlistener(m.bank_switch, func m.setbank(),0,0);
88         m.LTMode = setlistener(m.autothrottle_mode, func m.updateATMode(),0,0);
89         m.WpChanged = setlistener(props.globals.getNode("/autopilot/route-manager/wp/id",1), func m.wpChanged(),0,0);
90         m.RmDisabled = setlistener(props.globals.getNode("/autopilot/route-manager/active",1), func m.wpChanged(),0,0);
91         return m;
92     },
93
94 ####    Inputs    ####
95 ###################
96     input : func(mode,btn){
97         var fms = 0;
98         if(mode==0){
99             # horizontal AP controls
100             if(me.lateral_mode.getValue() ==btn) btn=0;
101             if (btn==2)
102             {
103                 if (me.AP.getValue() and (me.lateral_mode.getValue()!=1))
104                 {
105                     # set target to current magnetic heading
106                     var tgtHdg = int(getprop("orientation/heading-magnetic-deg") + 0.50);
107                     if (tgtHdg==0) tgtHdg=360;
108                     me.hdg_setting.setValue(tgtHdg);
109                     btn = 1;
110                 } else
111                     btn = 0;
112             }
113             if(btn==3)
114             {
115                 if ((!getprop("/autopilot/route-manager/active"))or
116                     (getprop("/autopilot/route-manager/current-wp")<0)or
117                     (getprop("/autopilot/route-manager/wp/id")==""))
118                 {
119                     # Oops, route manager isn't active. Keep current mode.
120                     btn = me.lateral_mode.getValue();
121                     copilot("Captain, LNAV doesn't engage. We forgot to program or activate the route manager!");
122                 }
123                 else
124                     fms=1;
125             }
126             me.lateral_mode.setValue(btn);
127         }elsif(mode==1){
128             # vertical AP controls
129             if(me.vertical_mode.getValue() ==btn) btn=0;
130             if (btn==1){
131                 # hold current altitude
132                 if (me.AP.getValue())
133                 {
134                     var alt = int((getprop("instrumentation/altimeter/indicated-altitude-ft")+50)/100)*100;
135                     me.alt_setting.setValue(alt);
136                 } else
137                     btn = 0;
138             }
139             if (btn==2){
140                 # hold current vertical speed
141                 if (me.AP.getValue())
142                 {
143                     var vs = getprop("instrumentation/inst-vertical-speed-indicator/indicated-speed-fpm");
144                     if (vs<0) vs -= 50;else vs+=50;
145                     vs = int(vs/100)*100;
146                     if (vs<-8000) vs = -8000;
147                     if (vs>6000) vs = 6000;
148                     me.vs_setting.setValue(vs);
149                 } else
150                     btn = 0;
151             }
152             if (btn==8)
153             {
154                 # change flight level
155             }
156             me.vertical_mode.setValue(btn);
157         }elsif(mode==2){
158             # throttle AP controls
159             if(me.autothrottle_mode.getValue() ==btn) btn=0;
160             if(btn and (getprop("position/altitude-agl-ft")<200))
161             {
162                 btn=0;
163                 copilot("Captain, auto-throttle won't engage below 200ft.");
164             } 
165             me.autothrottle_mode.setValue(btn);
166         }elsif(mode==3){
167             var arm = 1-((me.loc_armed.getValue() or (4==me.lateral_mode.getValue())));
168             if (btn==1){
169                 # toggle G/S and LOC arm
170                 var arm = arm or (1-(me.gs_armed.getValue() or (6==me.vertical_mode.getValue())));
171                 me.gs_armed.setValue(arm);
172                 if ((arm==0)and(6==me.vertical_mode.getValue())) me.vertical_mode.setValue(0);
173             }
174             me.loc_armed.setValue(arm);
175             if((arm==0)and(4==me.lateral_mode.getValue())) me.lateral_mode.setValue(0);
176         }
177     },
178 ###################
179     setAP : func{
180         var output=1-me.AP.getValue();
181         var disabled = me.AP_disengaged.getValue();
182         if((output==0)and(getprop("position/altitude-agl-ft")<200))
183         {
184             disabled = 1;
185             copilot("Captain, autopilot won't engage below 200ft.");
186         }
187         if((disabled)and(output==0)){output = 1;me.AP.setValue(0);}
188         if (output==1)
189         {
190             var msg="";
191             var msg2="";
192             var msg3="";
193             if (abs(getprop("controls/flight/rudder-trim"))>0.04)   msg  = "rudder";
194             if (abs(getprop("controls/flight/elevator-trim"))>0.04) msg2 = "pitch";
195             if (abs(getprop("controls/flight/aileron-trim"))>0.04)  msg3 = "aileron";
196             if (msg ~ msg2 ~ msg3 != "")
197             {
198                 if ((msg != "")and(msg2!=""))
199                     msg = msg ~ ", " ~ msg2;
200                 else
201                     msg = msg ~ msg2;
202                 if ((msg != "")and(msg3!=""))
203                     msg = msg ~ " and " ~ msg3;
204                 else
205                     msg = msg ~ msg3;
206                 copilot("Captain, autopilot disengaged. Careful, check " ~ msg ~ " trim!");
207             }
208         }
209         else
210             if(me.lateral_mode.getValue() != 3) me.input(0,1);
211         setprop("autopilot/internal/target-pitch-deg",0);
212         setprop("autopilot/internal/target-roll-deg",0);
213         me.AP_passive.setValue(output);
214     },
215 ###################
216     setbank : func{
217         var banklimit=me.bank_switch.getValue();
218         var lmt=25;
219         if(banklimit>0){lmt=banklimit * 5};
220         me.bank_max.setValue(lmt);
221         lmt = -1 * lmt;
222         me.bank_min.setValue(lmt);
223     },
224 ###################
225     updateATMode : func()
226     {
227         var idx=me.autothrottle_mode.getValue();
228         me.AP_speed_mode.setValue(me.spd_list[idx]);
229     },
230 #################
231     wpChanged : func{
232         if (((getprop("/autopilot/route-manager/wp/id")=="")or
233              (!getprop("/autopilot/route-manager/active")))and
234             (me.lateral_mode.getValue() == 3)and
235             me.AP.getValue())
236         {
237             # LNAV active, but route manager is disabled now => switch to HDG HOLD (current heading)
238             me.input(0,2);
239         }
240     },
241 #################
242     ap_update : func{
243         var VS =getprop("velocities/vertical-speed-fps");
244         var TAS =getprop("velocities/uBody-fps");
245         if(TAS < 10) TAS = 10;
246         if(VS < -200) VS=-200;
247         if (abs(VS/TAS)<=1)
248         {
249           var FPangle = math.asin(VS/TAS);
250           FPangle *=90;
251           setprop("autopilot/internal/fpa",FPangle);
252         }
253         var msg=" ";
254         if(me.FD.getValue())msg="FLT DIR";
255         if(me.AP.getValue())msg="A/P";
256         me.AP_annun.setValue(msg);
257         var tmp = abs(me.vs_setting.getValue());
258         me.vs_display.setValue(tmp);
259         tmp = abs(me.fpa_setting.getValue());
260         me.fpa_display.setValue(tmp);
261         msg="";
262         var hdgoffset = me.hdg_setting.getValue()-getprop("orientation/heading-magnetic-deg");
263         if(hdgoffset < -180) hdgoffset +=360;
264         if(hdgoffset > 180) hdgoffset +=-360;
265         setprop("autopilot/internal/fdm-heading-bug-error-deg",hdgoffset);
266         if(getprop("position/altitude-agl-ft")<200){
267             me.AP.setValue(0);
268             me.autothrottle_mode.setValue(0);
269         }
270
271         if(me.step==0){ ### glideslope armed ?###
272             msg="";
273             if(me.gs_armed.getValue()){
274                 msg="G/S";
275                 var gsdefl = getprop("instrumentation/nav/gs-needle-deflection");
276                 var gsrange = getprop("instrumentation/nav/gs-in-range");
277                 if ((gsdefl< 0.5 and gsdefl>-0.5)and
278                     gsrange)
279                 {
280                     me.vertical_mode.setValue(6);
281                     me.gs_armed.setValue(0);
282                 }
283             }
284             me.AP_pitch_arm.setValue(msg);
285
286         }elsif(me.step==1){ ### localizer armed ? ###
287             msg="";
288             if(me.loc_armed.getValue())
289             {
290                 msg="LOC";
291                 if (getprop("instrumentation/nav/signal-quality-norm") > 0.9)
292                 {
293                     var hddefl = getprop("instrumentation/nav/heading-needle-deflection");
294                     var vtemp = 9.9;
295                     if(!getprop("instrumentation/nav/nav-loc"))
296                     {
297                         var vspeed = getprop("instrumentation/airspeed-indicator/indicated-speed-kt");
298                         var vcourse = getprop("instrumentation/nav/heading-deg");
299                         var vdistance = getprop("instrumentation/nav/nav-distance");
300                         vtemp = getprop("orientation/heading-deg");
301                         vcourse = abs(vcourse - vtemp);
302                         if (vcourse <= 90)
303                             vtemp = vcourse*vspeed*vdistance/(10*200*1852*15);
304                         if(vtemp > 9.9)
305                             vtemp = 9.9;
306                     }
307                     if(abs(hddefl) < vtemp)
308                     {
309                         me.lateral_mode.setValue(4);
310                         me.loc_armed.setValue(0);
311                     }
312                 }
313             }
314             me.AP_roll_arm.setValue(msg);
315
316         }elsif(me.step==2){ ### check lateral modes  ###
317             var idx=me.lateral_mode.getValue();
318             if ((idx == 1)or(idx == 2))
319             {
320                 # switch between HDG SEL to HDG HOLD
321                 if (abs(getprop("orientation/heading-magnetic-deg")-me.hdg_setting.getValue())<2)
322                     idx = 2; # HDG HOLD
323                 else
324                     idx = 1; # HDG SEL
325                 me.lateral_mode.setValue(idx);
326             }
327             me.AP_roll_mode.setValue(me.roll_list[idx]);
328             me.AP_roll_engaged.setBoolValue(idx>0);
329
330         }elsif(me.step==3){ ### check vertical modes  ###
331             var idx=me.vertical_mode.getValue();
332             var test_fpa=me.vs_fpa_selected.getValue();
333             if(idx==2 and test_fpa)idx=9;
334             if(idx==9 and !test_fpa)idx=2;
335             if ((idx==8)or(idx==1))
336             {
337                 # flight level change mode
338                 if (abs(getprop("instrumentation/altimeter/indicated-altitude-ft")-me.alt_setting.getValue())<50)
339                     # within target altitude: switch to ALT HOLD mode
340                     idx=1;
341                 else
342                     # outside target altitude: change flight level
343                     idx=8;
344                 me.vertical_mode.setValue(idx);
345             }
346             me.AP_pitch_mode.setValue(me.pitch_list[idx]);
347             me.AP_pitch_engaged.setBoolValue(idx>0);
348
349         }elsif(me.step==4){             ### check speed modes  ###
350             if (getprop("controls/engines/engine/reverser")) {
351                 # auto-throttle disables when reverser is enabled
352                 me.autothrottle_mode.setValue(0);
353             }
354         }
355
356         me.step+=1;
357         if(me.step>4)me.step =0;
358     },
359 };
360 #####################
361
362
363 var afds = AFDS.new();
364
365 setlistener("/sim/signals/fdm-initialized", func {
366     settimer(update_afds,5);
367     print("AFDS System ... check");
368 });
369
370 var update_afds = func {
371     afds.ap_update();
372
373 settimer(update_afds, 0);
374 }