Primus , autopilot and autopilot dialog updates ...
[fg:toms-fgdata.git] / Aircraft / Citation-Bravo / Nasal / flightdirector.nas
1 #############################################################################
2 # Flight Director/Autopilot controller.
3 # Syd Adams
4 #
5 # HDG:
6 # Heading Bug hold - Low Bank can be selected
7 # NAV:
8 # Arm & Capture VOR , LOC or FMS
9 # APR : (ILS approach)
10 # Arm & Capture VOR APR , LOC or BC
11 # Also arm and capture GS
12 # BC :
13 # Arm & capture localizer backcourse
14 # Nav also illuminates
15 # VNAV:
16 # Arm and capture VOR/DME or FMS vertical profile
17 # profile entered in MFD VNAV menu
18 # ALT:
19 # Hold current Altitude or PFD preset altitude
20 # VS:
21 # Hold current vertical speed
22 # adjustable with pitch wheel
23 # SPD :
24 # Hold current speed 
25 # adjustable with pitch wheel
26 #
27 #############################################################################
28 # lnav 
29 #0=W-LVL , 1=HDG , 2=NAV Arm ,3=NAV Cap , 4=LOC arm , 5=LOC Cap , 6=FMS
30 # vnav
31 # 0=PITCH,  1=VNAV, 2=ALT hold , 3=VS , 4=GS arm ,5 = GS cap
32 #FlightDirector/Autopilot 
33 # ie: var fltdir = flightdirector.new(property);
34
35 var ap_settings = gui.Dialog.new("/sim/gui/dialogs/primus-autopilot/dialog",
36         "Aircraft/Citation-Bravo/Systems/autopilot-dlg.xml");
37
38 var flightdirector = {
39     new : func(fdprop){
40         m = {parents : [flightdirector]};
41         m.lnav_text=["ROLL","HDG","NAV-ARM","NAV","LOC-ARM","LOC","LNAV"];
42         m.vnav_text=["PTCH","VNAV","ALT","VS","GS-ARM","GS"];
43         m.spd_text=["","IAS","speed-with-pitch"];
44         m.LAT=["ROL","HDG","HDG","VOR","HDG","LOC","LNAV"];
45         m.subLAT=["   ","   ","VOR","   ","LOC","   ","   "];
46         m.VRT=["PIT","VNAV","ALT","VS","   ","GS"];
47     m.subVRT=["   ","GS"];
48     m.node = props.globals.getNode(fdprop,1);
49         m.yawdamper = props.globals.getNode("autopilot/locks/yaw-damper",1);
50         m.yawdamper.setBoolValue(0);
51         m.lnav = m.node.getNode("lnav",1);
52         m.lnav.setIntValue(0);
53         m.vnav = m.node.getNode("vnav",1);
54         m.vnav.setIntValue(0);
55         m.gs_arm = m.node.getNode("gs-arm",1);
56         m.gs_arm.setBoolValue(0);
57         m.asel = m.node.getNode("Asel",1);
58         m.asel.setDoubleValue(0);
59         m.speed = m.node.getNode("spd",1);
60         m.speed.setDoubleValue(0);
61         m.crs = m.node.getNode("crs",1);
62         m.crs.setDoubleValue(0);
63         m.Defl = m.node.getNode("crs-deflection",1);
64         m.Defl.setDoubleValue(0);
65         m.DH= props.globals.getNode("instrumentation/mk-viii/inputs/arinc429/decision-height",1);
66         m.GSDefl = m.node.getNode("gs-deflection",1);
67         m.GSDefl.setDoubleValue(0);
68         m.NavLoc = m.node.getNode("localizer",1);
69         m.NavLoc.setBoolValue(0);
70         m.hasGS = m.node.getNode("glideslope",1);
71         m.hasGS.setBoolValue(0);
72         m.navValid = m.node.getNode("in-range",1);
73         m.navValid.setBoolValue(0);
74         m.navDist = props.globals.getNode("instrumentation/primus1000/nav-dist-nm",1);
75         m.navCRS = m.node.getNode("nav-crs-offset",1);
76         m.navCRS.setDoubleValue(0);
77         m.FMS = props.globals.getNode("instrumentation/primus1000/dc550/fms",1);
78         m.NAV = props.globals.getNode("instrumentation/primus1000/dc550/nav",1);
79         m.AP_hdg = props.globals.getNode("/autopilot/locks/heading",1);
80         m.AP_hdg.setValue(m.lnav_text[0]);
81         m.AP_hdg_setting = props.globals.getNode("/autopilot/settings/heading",1);
82         m.AP_hdg_setting.setDoubleValue(0);
83         m.AP_spd_setting = props.globals.getNode("/autopilot/settings/target-speed-kt",1);
84         m.AP_spd_setting.setDoubleValue(0);
85         m.AP_alt = props.globals.getNode("/autopilot/locks/altitude",1);
86         m.AP_alt.setValue(m.vnav_text[0]);
87         m.AP_spd = props.globals.getNode("/autopilot/locks/speed",1);
88         m.AP_spd.setValue(m.spd_text[0]);
89         m.AP_off = props.globals.getNode("/autopilot/locks/passive-mode",1);
90         m.AP_off.setBoolValue(1);
91     
92     m.AP_lat_annun = m.node.getNode("LAT-annun",1);
93         m.AP_lat_annun.setValue(" ");
94     m.AP_sublat_annun = m.node.getNode("LAT-arm-annun",1);
95         m.AP_sublat_annun.setValue(" ");
96     m.AP_vert_annun = m.node.getNode("VRT-annun",1);
97         m.AP_vert_annun.setValue(" ");
98     m.AP_subvert_annun = m.node.getNode("VRT-arm-annun",1);
99         m.AP_subvert_annun.setValue(" ");
100
101         m.pitch_active=props.globals.getNode("/autopilot/locks/pitch-active",1);
102         m.pitch_active.setBoolValue(1);
103         m.roll_active=props.globals.getNode("/autopilot/locks/roll-active",1);
104         m.roll_active.setBoolValue(1);
105         m.bank_limit=m.node.getNode("bank-limit-switch",1);
106         m.bank_limit.setBoolValue(0);
107
108         m.max_pitch=m.node.getNode("pitch-max",1);
109         m.max_pitch.setDoubleValue(10);
110         m.min_pitch=m.node.getNode("pitch-min",1);
111         m.min_pitch.setDoubleValue(-10);
112         m.max_roll=m.node.getNode("roll-max",1);
113         m.max_roll.setDoubleValue(27);
114         m.min_roll=m.node.getNode("roll-min",1);
115         m.min_roll.setDoubleValue(-27);
116     return m;
117     },
118     ############################
119     set_lateral_mode : func(lnv){
120     var tst =me.lnav.getValue();
121     if(lnv ==tst)lnv=0;
122         if(lnv==4){
123             if(!me.NavLoc.getBoolValue()){
124                 lnv=2;
125             }else{
126                 if(me.hasGS.getBoolValue())me.gs_arm.setBoolValue(1);
127             }
128         }
129         if(lnv==2){
130             if(me.FMS.getBoolValue())lnv = 6;
131             }
132         me.lnav.setValue(lnv);
133         me.AP_hdg.setValue(me.lnav_text[lnv]);
134     },
135 ###########################
136     set_vertical_mode : func(vnv){
137     var tst =me.vnav.getValue();
138     if(vnv ==tst)vnv=0;
139         if(vnv==1){
140             if(!me.FMS.getBoolValue()){
141                 vnv = 0;
142             }else{
143                 var tmpalt =getprop("autopilot/route-manager/route/wp/altitude-ft");
144                 if(tmpalt < 0)tmpalt=30000;
145                 setprop("autopilot/settings/target-altitude-ft",tmpalt);
146             }
147         }
148         if(vnv==2){
149         var asel=getprop("instrumentation/altimeter/indicated-altitude-ft");
150         asel = int(asel * 0.01) * 100;
151         setprop("autopilot/settings/target-altitude-ft",asel);
152         }
153         me.vnav.setValue(vnv);
154         me.AP_alt.setValue(me.vnav_text[vnv]);
155     },
156 ###########################
157     set_course : func(crs){
158         var rd =0;
159         var nvnum =getprop("instrumentation/primus1000/dc550/nav");
160         if(nvnum == nil)nvnum=0;
161         if(!me.FMS.getBoolValue()){
162             rd = getprop("instrumentation/nav["~nvnum~"]/radials/selected-deg");
163             if(crs ==0){
164                 rd=int(getprop("orientation/heading-magnetic-deg"));
165             }else{
166                 rd = rd+crs;
167                 if(rd >360)rd =rd-360;
168                 if(rd <1)rd = rd +360;
169             }
170             setprop("instrumentation/nav["~nvnum~"]/radials/selected-deg",rd);
171         }
172     },
173 ###########################
174     set_hdg_bug : func(hbg){
175         var rd =0;
176             rd = getprop("autopilot/settings/heading-bug-deg");
177             if(rd==nil)rd=0;
178             if(hbg ==0){
179                 rd=int(getprop("orientation/heading-magnetic-deg"));
180             }else{
181                 rd = rd+hbg;
182                 if(rd >360)rd =rd-360;
183                 if(rd <1)rd = rd +360;
184             }
185             setprop("autopilot/settings/heading-bug-deg",rd);
186     },
187 ###########################
188     ias_set : func(spd){
189         var rd =0;
190             rd = me.AP_spd_setting.getValue();
191             if(rd==nil)rd=0;
192             if(spd ==0){
193                 rd=0;
194             }else{
195                 rd = rd+spd;
196                 if(rd >400)rd =400;
197                 if(rd <0)rd = 0;
198             }
199             me.AP_spd_setting.setValue(rd);
200     },
201 #### button press handler####
202     set_mode : func(mode){
203         if(mode=="hdg"){
204             me.set_lateral_mode(1);
205         }elsif(mode=="nav"){
206             me.set_lateral_mode(2);
207         }elsif(mode=="apr"){
208             me.set_lateral_mode(4);
209         }elsif(mode=="bc"){
210             var tst=me.lnav.getValue();
211             var bcb = getprop("instrumentation/nav/back-course-btn");
212             bcb = 1-bcb;
213             if(tst <2 and tst >5)bcb = 0;
214             setprop("instrumentation/nav/back-course-btn",bcb);
215         }elsif(mode=="vnav"){
216             me.set_vertical_mode(1);
217         }elsif(mode=="alt"){
218             me.set_vertical_mode(2);
219         }elsif(mode=="vs"){
220             me.set_vertical_mode(3);
221         }elsif(mode=="ias"){
222             var sp=me.speed.getValue();
223             sp=1-sp;
224             me.speed.setValue(sp);
225             var kt= int(getprop("velocities/airspeed-kt"));
226             me.AP_spd_setting.setValue(kt);
227             me.AP_spd.setValue(me.spd_text[sp]);
228         }
229     },
230 #### check AP errors####
231     check_AP_limits : func(){
232         var apmode = me.AP_off.getBoolValue();
233         var navunit =me.NAV.getValue();
234         me.nav_crs(navunit);
235         var tmp_nav="instrumentation/nav["~navunit~"]/";
236             me.crs.setValue(getprop(tmp_nav~"radials/selected-deg"));
237             me.Defl.setValue(getprop(tmp_nav~"heading-needle-deflection"));
238             me.GSDefl.setValue(getprop(tmp_nav~"gs-needle-deflection"));
239         if(getprop(tmp_nav~"data-is-valid")){
240             me.NavLoc.setValue(getprop(tmp_nav~"nav-loc"));
241             me.hasGS.setValue(getprop(tmp_nav~"has-gs"));
242             me.navValid.setValue(getprop(tmp_nav~"in-range"));
243          }else{
244             me.NavLoc.setValue(0);
245             me.hasGS.setValue(0);
246             me.navValid.setValue(0);
247         }
248
249         var agl=getprop("/position/altitude-agl-ft");
250         if(!apmode){
251             var maxroll = getprop("/orientation/roll-deg");
252             var maxpitch = getprop("/orientation/pitch-deg");
253             if(maxroll > 65 or maxroll < -65){
254                 apmode = 1;
255             }
256             if(maxpitch > 30 or maxpitch < -20){
257                 apmode = 1;
258                 setprop("controls/flight/elevator-trim",0);
259             }
260             if(agl < 180)apmode = 1;
261             me.AP_off.setBoolValue(apmode);
262         }
263         if(agl < 50)me.yawdamper.setBoolValue(0);
264         return apmode;
265     },
266 #### update lnav####
267     nav_crs : func(unit){
268     var  hdg= me.crs.getValue() -getprop("orientation/heading-magnetic-deg");
269     hdg+=me.Defl.getValue()*3;
270     if(hdg>180)hdg-=360;
271     if(hdg<-180)hdg+=360;
272     me.navCRS.setValue(hdg);
273     },
274 #### update lnav####
275     update_lnav : func(){
276         var lnv = me.lnav.getValue();
277         var valid=me.navValid.getBoolValue();
278         if(lnv   >1 and lnv<6){
279         if(me.FMS.getBoolValue())lnv=6;
280     }
281     if(lnv==2){
282         var defl = me.Defl.getValue();
283         if(valid){
284             if(defl <= 9 and defl >= -9)lnv=3;
285         }
286     }elsif(lnv==4){
287         var defl = me.Defl.getValue();
288         if(valid){
289             if(defl <= 9 and defl >= -9)lnv=5;
290         }
291     }
292     me.lnav.setValue(lnv);
293         me.AP_hdg.setValue(me.lnav_text[lnv]);
294     me.AP_lat_annun.setValue(me.LAT[lnv]);
295     me.AP_sublat_annun.setValue(me.subLAT[lnv]);
296     },
297 #### update vnav####
298     update_vnav : func(){
299         var vnv = me.vnav.getValue();
300         if(me.gs_arm.getBoolValue()){
301             if(me.lnav.getValue() ==5 and me.navDist.getValue() <30){
302                 var defl = me.GSDefl.getValue();
303                 if(defl < 1 and defl > -1){
304                     vnv=5;
305                     me.gs_arm.setBoolValue(0);
306                 }
307             }
308         }
309     me.vnav.setValue(vnv);
310         me.AP_alt.setValue(me.vnav_text[vnv]);
311     me.AP_vert_annun.setValue(me.VRT[vnv]);
312     me.AP_subvert_annun.setValue(me.subVRT[me.gs_arm.getValue()]);
313     },
314 #### autopilot engage####
315     toggle_autopilot : func(apmd){
316         var md1=0;
317         if(apmd=="ap"){
318             md1 = me.AP_off.getBoolValue();
319             md1=1-md1;
320             if(getprop("/position/altitude-agl-ft") < 180)md1=1;
321             me.AP_off.setBoolValue(md1);
322             if(md1==0)me.yawdamper.setBoolValue(1);
323         }elsif(apmd=="yd"){
324             md1 = me.yawdamper.getBoolValue();
325             md1=1-md1;
326             me.yawdamper.setBoolValue(md1);
327             if(md1==0)me.AP_off.setBoolValue(1);
328         }elsif(apmd=="bank"){
329             md1 = me.bank_limit.getBoolValue();
330             md1=1-md1;
331             me.bank_limit.setBoolValue(md1);
332             if(md1==1){
333                 me.max_roll.setValue(14);
334                 me.min_roll.setValue(-14);
335             }else{
336                 me.max_roll.setValue(27);
337                 me.min_roll.setValue(-27);
338             }
339         }
340     },
341 #### pitch wheel####
342     pitch_wheel : func(amt){
343         var factor=amt;
344         var vmd = me.vnav.getValue();
345         var ptc=0;
346             var mx=0;
347             var mn=0;
348         if(vmd==0){
349             mx=me.max_pitch.getValue();
350             mn=me.min_pitch.getValue();
351             ptc = getprop("autopilot/settings/target-pitch-deg");
352             if(ptc==nil)ptc=0;
353             ptc=ptc+0.10 *  amt;
354             if(ptc>mx)ptc=mx;
355             if(ptc<mn)ptc=mn;
356             setprop("autopilot/settings/target-pitch-deg",ptc);
357         }elsif(vmd==3){
358             mx=6000;
359             mn=-6000;
360             ptc = getprop("autopilot/settings/vertical-speed-fpm");
361             if(ptc==nil)ptc=0;
362             ptc=ptc+100 *amt;
363             if(ptc>mx)ptc=mx;
364             if(ptc<mn)ptc=mn;
365             setprop("autopilot/settings/vertical-speed-fpm",ptc);
366         }
367     },
368 #### altitude ###
369     preset_altitude : func(alt){
370         if(alt==0){
371             setprop("autopilot/settings/target-altitude-ft",0);
372         }else{
373             var asel =getprop("autopilot/settings/target-altitude-ft");
374             asel +=alt;
375             if(asel<0)asel=0;
376             if(asel>50000)asel=50000;
377             setprop("autopilot/settings/target-altitude-ft",asel);
378         }
379     }
380 };
381
382 var FlDr=flightdirector.new("instrumentation/flightdirector");
383
384 ######################################
385
386 setlistener("/sim/signals/fdm-initialized", func {
387     setprop("autopilot/settings/target-altitude-ft",10000);
388     setprop("autopilot/settings/heading-bug-deg",0);
389     setprop("autopilot/settings/vertical-speed-fpm",0);
390     setprop("autopilot/settings/target-pitch-deg",0);
391     settimer(update_fd, 5);
392     print("Flight Director ...Check");
393 });
394
395 var update_fd = func {
396 var APoff = FlDr.check_AP_limits();
397 FlDr.update_lnav();
398 FlDr.update_vnav();
399 settimer(update_fd, 0); 
400 }