Primus, radar 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.DH = props.globals.getNode("autopilot/route-manager/min-lock-altitude-agl-ft",1);
62         m.Defl = props.globals.getNode("instrumentation/nav/heading-needle-deflection");
63         m.GSDefl = props.globals.getNode("instrumentation/nav/gs-needle-deflection");
64         m.NavLoc = props.globals.getNode("instrumentation/nav/nav-loc");
65         m.hasGS = props.globals.getNode("instrumentation/nav/has-gs");
66         m.Valid = props.globals.getNode("instrumentation/nav/in-range");
67         m.FMS = props.globals.getNode("instrumentation/primus1000/dc550/fms",1);
68         m.NAV = props.globals.getNode("instrumentation/primus1000/dc550/nav",1);
69         m.AP_hdg = props.globals.getNode("/autopilot/locks/heading",1);
70         m.AP_hdg.setValue(m.lnav_text[0]);
71         m.AP_hdg_setting = props.globals.getNode("/autopilot/settings/heading",1);
72         m.AP_hdg_setting.setDoubleValue(0);
73         m.AP_spd_setting = props.globals.getNode("/autopilot/settings/target-speed-kt",1);
74         m.AP_spd_setting.setDoubleValue(0);
75         m.AP_alt = props.globals.getNode("/autopilot/locks/altitude",1);
76         m.AP_alt.setValue(m.vnav_text[0]);
77         m.AP_spd = props.globals.getNode("/autopilot/locks/speed",1);
78         m.AP_spd.setValue(m.spd_text[0]);
79         m.AP_off = props.globals.getNode("/autopilot/locks/passive-mode",1);
80         m.AP_off.setBoolValue(1);
81     
82     m.AP_lat_annun = m.node.getNode("LAT-annun",1);
83         m.AP_lat_annun.setValue(" ");
84     m.AP_sublat_annun = m.node.getNode("LAT-arm-annun",1);
85         m.AP_sublat_annun.setValue(" ");
86     m.AP_vert_annun = m.node.getNode("VRT-annun",1);
87         m.AP_vert_annun.setValue(" ");
88     m.AP_subvert_annun = m.node.getNode("VRT-arm-annun",1);
89         m.AP_subvert_annun.setValue(" ");
90
91         m.pitch_active=props.globals.getNode("/autopilot/locks/pitch-active",1);
92         m.pitch_active.setBoolValue(1);
93         m.roll_active=props.globals.getNode("/autopilot/locks/roll-active",1);
94         m.roll_active.setBoolValue(1);
95         m.bank_limit=m.node.getNode("bank-limit-switch",1);
96         m.bank_limit.setBoolValue(0);
97
98         m.max_pitch=m.node.getNode("pitch-max",1);
99         m.max_pitch.setDoubleValue(10);
100         m.min_pitch=m.node.getNode("pitch-min",1);
101         m.min_pitch.setDoubleValue(-10);
102         m.max_roll=m.node.getNode("roll-max",1);
103         m.max_roll.setDoubleValue(27);
104         m.min_roll=m.node.getNode("roll-min",1);
105         m.min_roll.setDoubleValue(-27);
106     return m;
107     },
108     ############################
109     set_lateral_mode : func(lnv){
110     var tst =me.lnav.getValue();
111     if(lnv ==tst)lnv=0;
112         if(lnv==4){
113             if(!me.NavLoc.getBoolValue()){
114                 lnv=2;
115             }else{
116                 if(me.hasGS.getBoolValue())me.gs_arm.setBoolValue(1);
117             }
118         }
119         if(lnv==2){
120             if(me.FMS.getBoolValue())lnv = 6;
121             }
122         me.lnav.setValue(lnv);
123         me.AP_hdg.setValue(me.lnav_text[lnv]);
124     },
125 ###########################
126     set_vertical_mode : func(vnv){
127     var tst =me.vnav.getValue();
128     if(vnv ==tst)vnv=0;
129         if(vnv==1){
130             if(!me.FMS.getBoolValue()){
131                 vnv = 0;
132             }else{
133                 var tmpalt =getprop("autopilot/route-manager/route/wp/altitude-ft");
134                 if(tmpalt < 0)tmpalt=30000;
135                 setprop("autopilot/settings/target-altitude-ft",tmpalt);
136             }
137         }
138         if(vnv==2){
139         var asel=getprop("instrumentation/altimeter/indicated-altitude-ft");
140         asel = int(asel * 0.01) * 100;
141         setprop("autopilot/settings/target-altitude-ft",asel);
142         }
143         me.vnav.setValue(vnv);
144         me.AP_alt.setValue(me.vnav_text[vnv]);
145     },
146 ###########################
147     set_course : func(nvnum,crs){
148         var rd =0;
149         if(!me.FMS.getBoolValue()){
150             rd = getprop("instrumentation/nav["~nvnum~"]/radials/selected-deg");
151             if(crs ==0){
152                 rd=int(getprop("orientation/heading-magnetic-deg"));
153             }else{
154                 rd = rd+crs;
155                 if(rd >360)rd =rd-360;
156                 if(rd <1)rd = rd +360;
157             }
158             setprop("instrumentation/nav["~nvnum~"]/radials/selected-deg",rd);
159         }
160     },
161 ###########################
162     set_hdg_bug : func(hbg){
163         var rd =0;
164             rd = getprop("autopilot/settings/heading-bug-deg");
165             if(rd==nil)rd=0;
166             if(hbg ==0){
167                 rd=int(getprop("orientation/heading-magnetic-deg"));
168             }else{
169                 rd = rd+hbg;
170                 if(rd >360)rd =rd-360;
171                 if(rd <1)rd = rd +360;
172             }
173             setprop("autopilot/settings/heading-bug-deg",rd);
174     },
175 ###########################
176     ias_set : func(spd){
177         var rd =0;
178             rd = me.AP_spd_setting.getValue();
179             if(rd==nil)rd=0;
180             if(spd ==0){
181                 rd=0;
182             }else{
183                 rd = rd+spd;
184                 if(rd >400)rd =400;
185                 if(rd <0)rd = 0;
186             }
187             me.AP_spd_setting.setValue(rd);
188     },
189 #### button press handler####
190     set_mode : func(mode){
191         if(mode=="hdg"){
192             me.set_lateral_mode(1);
193         }elsif(mode=="nav"){
194             me.set_lateral_mode(2);
195         }elsif(mode=="apr"){
196             me.set_lateral_mode(4);
197         }elsif(mode=="bc"){
198             var tst=me.lnav.getValue();
199             var bcb = getprop("instrumentation/nav/back-course-btn");
200             bcb = 1-bcb;
201             if(tst <2 and tst >5)bcb = 0;
202             setprop("instrumentation/nav/back-course-btn",bcb);
203         }elsif(mode=="vnav"){
204             me.set_vertical_mode(1);
205         }elsif(mode=="alt"){
206             me.set_vertical_mode(2);
207         }elsif(mode=="vs"){
208             me.set_vertical_mode(3);
209         }elsif(mode=="ias"){
210             var sp=me.speed.getValue();
211             sp=1-sp;
212             me.speed.setValue(sp);
213             var kt= int(getprop("velocities/airspeed-kt"));
214             me.AP_spd_setting.setValue(kt);
215             me.AP_spd.setValue(me.spd_text[sp]);
216         }
217     },
218 #### check AP errors####
219     check_AP_limits : func(){
220         var apmode = me.AP_off.getBoolValue();
221         var agl=getprop("/position/altitude-agl-ft");
222         if(!apmode){
223             var maxroll = getprop("/orientation/roll-deg");
224             var maxpitch = getprop("/orientation/pitch-deg");
225             if(maxroll > 65 or maxroll < -65){
226                 apmode = 1;
227             }
228             if(maxpitch > 30 or maxpitch < -20){
229                 apmode = 1;
230                 setprop("controls/flight/elevator-trim",0);
231             }
232             if(agl < 180)apmode = 1;
233             me.AP_off.setBoolValue(apmode);
234         }
235         if(agl < 50)me.yawdamper.setBoolValue(0);
236         return apmode;
237     },
238 #### update lnav####
239     update_lnav : func(){
240         var lnv = me.lnav.getValue();
241         if(lnv   >1 and lnv<6){
242         if(me.FMS.getBoolValue())lnv=6;
243     }
244     if(lnv==2){
245         var defl = me.Defl.getValue();
246         if(me.Valid.getBoolValue()){
247             if(defl <= 9 and defl >= -9)lnv=3;
248         }
249     }elsif(lnv==4){
250         var defl = me.Defl.getValue();
251         if(me.Valid.getBoolValue()){
252             if(defl <= 9 and defl >= -9)lnv=5;
253         }
254     }
255     me.lnav.setValue(lnv);
256         me.AP_hdg.setValue(me.lnav_text[lnv]);
257     me.AP_lat_annun.setValue(me.LAT[lnv]);
258     me.AP_sublat_annun.setValue(me.subLAT[lnv]);
259     },
260 #### update vnav####
261     update_vnav : func(){
262         var vnv = me.vnav.getValue();
263         if(me.gs_arm.getBoolValue()){
264             var defl = me.GSDefl.getValue();
265             if(defl < 1 and defl > -1){
266         vnv=5;
267         me.gs_arm.setBoolValue(0);
268         }
269         }
270     me.vnav.setValue(vnv);
271         me.AP_alt.setValue(me.vnav_text[vnv]);
272     me.AP_vert_annun.setValue(me.VRT[vnv]);
273     me.AP_subvert_annun.setValue(me.subVRT[me.gs_arm.getValue()]);
274     },
275 #### autopilot engage####
276     toggle_autopilot : func(apmd){
277         var md1=0;
278         if(apmd=="ap"){
279             md1 = me.AP_off.getBoolValue();
280             md1=1-md1;
281             if(getprop("/position/altitude-agl-ft") < 180)md1=1;
282             me.AP_off.setBoolValue(md1);
283             if(md1==0)me.yawdamper.setBoolValue(1);
284         }elsif(apmd=="yd"){
285             md1 = me.yawdamper.getBoolValue();
286             md1=1-md1;
287             me.yawdamper.setBoolValue(md1);
288             if(md1==0)me.AP_off.setBoolValue(1);
289         }elsif(apmd=="bank"){
290             md1 = me.bank_limit.getBoolValue();
291             md1=1-md1;
292             me.bank_limit.setBoolValue(md1);
293             if(md1==1){
294                 me.max_roll.setValue(14);
295                 me.min_roll.setValue(-14);
296             }else{
297                 me.max_roll.setValue(27);
298                 me.min_roll.setValue(-27);
299             }
300         }
301     },
302 #### pitch wheel####
303     pitch_wheel : func(amt){
304         var factor=amt;
305         var vmd = me.vnav.getValue();
306         var ptc=0;
307             var mx=0;
308             var mn=0;
309         if(vmd==0){
310             mx=me.max_pitch.getValue();
311             mn=me.min_pitch.getValue();
312             ptc = getprop("autopilot/settings/target-pitch-deg");
313             if(ptc==nil)ptc=0;
314             ptc=ptc+0.10 *  amt;
315             if(ptc>mx)ptc=mx;
316             if(ptc<mn)ptc=mn;
317             setprop("autopilot/settings/target-pitch-deg",ptc);
318         }elsif(vmd==3){
319             mx=6000;
320             mn=-6000;
321             ptc = getprop("autopilot/settings/vertical-speed-fpm");
322             if(ptc==nil)ptc=0;
323             ptc=ptc+100 *amt;
324             if(ptc>mx)ptc=mx;
325             if(ptc<mn)ptc=mn;
326             setprop("autopilot/settings/vertical-speed-fpm",ptc);
327         }
328     },
329 #### roll knob ###
330     roll_knob : func(rl){
331     }
332 };
333
334 var FlDr=flightdirector.new("instrumentation/flightdirector");
335
336 ######################################
337
338 setlistener("/sim/signals/fdm-initialized", func {
339     setprop("autopilot/settings/target-altitude-ft",10000);
340     setprop("autopilot/settings/heading-bug-deg",0);
341     setprop("autopilot/settings/vertical-speed-fpm",0);
342     setprop("autopilot/settings/target-pitch-deg",0);
343     settimer(update_fd, 5);
344     print("Flight Director ...Check");
345 });
346
347 var update_fd = func {
348 var APoff = FlDr.check_AP_limits();
349 FlDr.update_lnav();
350 FlDr.update_vnav();
351 settimer(update_fd, 0); 
352 }