fixed some nasal errors ...
[fg:toms-fgdata.git] / Aircraft / Citation-Bravo / Nasal / flightdirector.nas
1 #############################################################################
2 # Flight Director/Autopilot controller.
3 #Syd Adams
4 #############################################################################
5
6 # 0 - Off: v-bars hidden
7 # lnav -0=W-LVL,1=HDG,2=VOR,3=LOC,4=LNAV,5=VAPP,6=BC
8 # vnav - 0=PIT,1=ALT,2=ASEL,3=GA,4=GS, 5= VNAV,6 = VS,7=FLC
9 var MTR2KT=0.000539956803;
10 var GPS_CDI=props.globals.getNode("/instrumentation/gps/cdi-deflection",1);
11 var GO = 0;
12 var alt_select = 0.0;
13 var current_alt=0.0;
14 var current_heading = 0.0;
15 var lnav = 0.0;
16 var vnav = 0.0;
17 var spd = 0.0;
18 var alt_alert = 0.0;
19 var course = 0.0;
20 var slaved = 0;
21 lnav_text=["wing-leveler","dg-heading-hold","nav1-hold","nav1-hold","true-heading-hold","nav1-hold","nav1-hold"];
22 vnav_text=["pitch-hold","altitude-hold","altitude-hold","pitch-hold","gs1-hold","altitude-hold","vertical-speed-hold","altitude-hold"];
23 var mag_offset=0;
24 var lMode=[" ","HDG","VOR","LOC","LNAV","VAPP","BC"];
25 var vMode=[" ","ALT","ASEL","GA","GS","VNAV","VS","FLC"];
26 var FMS = props.globals.getNode("/instrumentation/primus1000/dc550",1);
27 AP_hdg = props.globals.getNode("/autopilot/locks/heading",1);
28 AP_alt = props.globals.getNode("/autopilot/locks/altitude",1);
29 AP_spd = props.globals.getNode("/autopilot/locks/speed",1);
30 AP_lnav = props.globals.getNode("/instrumentation/flightdirector/lnav",1);
31 FD_deflection = props.globals.getNode("/instrumentation/flightdirector/crs-deflection",1);
32 FD_heading = props.globals.getNode("/instrumentation/flightdirector/hdg-deg",1);
33 HDG_deflection = props.globals.getNode("/instrumentation/nav/heading-needle-deflection",1);
34 AP_vnav = props.globals.getNode("/instrumentation/flightdirector/vnav",1);
35 AP_speed = props.globals.getNode("/instrumentation/flightdirector/spd",1);
36 AP_passive = props.globals.getNode("/autopilot/locks/passive-mode",1);
37 BC_btn = props.globals.getNode("/instrumentation/nav/back-course-btn",1); 
38 GPS_on = props.globals.getNode("/instrumentation/gps/serviceable",1);
39 WP1 = props.globals.getNode("/instrumentation/gps/wp/wp[1]",1);
40 RADIAL =props.globals.getNode("/instrumentation/nav/radials/selected-deg",1);
41 NAV_BRG = props.globals.getNode("/instrumentation/flightdirector/nav-mag-brg",1);
42
43 setlistener("/sim/signals/fdm-initialized", func {
44     current_alt = props.globals.getNode("/instrumentation/altimeter/indicated-altitude-ft").getValue();
45     alt_select = props.globals.getNode("/autopilot/settings/target-altitude-ft").getValue();
46     AP_speed.setValue(0);
47     AP_lnav.setValue(0);
48     AP_vnav.setValue(0);
49     AP_passive.setBoolValue(1);
50     BC_btn.setValue(0);
51     course = props.globals.getNode("/instrumentation/nav/radials/selected-deg").getValue();
52     slaved = props.globals.getNode("/instrumentation/nav/slaved-to-gps").getBoolValue();
53     props.globals.getNode("instrumentation/gps/wp/wp[0]/desired-course-deg").setValue(course);
54     props.globals.getNode("instrumentation/gps/wp/wp[1]/desired-course-deg").setValue(course);
55     GPS_on.setBoolValue(1);
56     mag_offset=props.globals.getNode("/environment/magnetic-variation-deg").getValue();
57     GO = 1;
58     settimer(update, 2);
59     print("Flight Director ...Check");
60 });
61
62 ####    Mode Controller inputs    ####
63
64 setlistener("/instrumentation/flightdirector/lnav", func(ln){
65     lnav = ln.getValue();
66     var Vn=getprop("/instrumentation/flightdirector/vnav");
67     if(Vn==nil){Vn=0;}
68     if(lnav == 0 or lnav ==nil){
69         BC_btn.setBoolValue(0);
70     }
71     if(lnav == 1){
72         BC_btn.setBoolValue(0);
73         if(Vn ==4 ){Vn = 0;}
74     }
75     if(lnav == 2){
76         if(getprop("/instrumentation/primus1000/dc550/fms")){
77         lnav=4;
78         }
79     }
80     if(lnav == 3){BC_btn.setBoolValue(0);
81         if(!getprop("instrumentation/nav/nav-loc")){
82             lnav=2;
83         }
84     }
85     if(lnav == 5){BC_btn.setBoolValue(0);
86         if(getprop("instrumentation/nav/has-gs")){
87             Vn=4;
88         }
89     }
90     if(lnav == 6){BC_btn.setBoolValue(1);
91         if(Vn==4){Vn = 0;}
92     }
93     AP_hdg.setValue(lnav_text[lnav]);
94     setprop("instrumentation/flightdirector/lateral-mode",lMode[lnav]);
95     setprop("instrumentation/flightdirector/vnav",Vn);
96 },0,0);
97
98 setlistener("/instrumentation/flightdirector/vnav", func(vn){
99     vnav = vn.getValue();
100     if(vnav == 4){
101         if (!getprop("/instrumentation/nav/has-gs",1)){
102             vnav = 0;
103         }
104     }
105     AP_alt.setValue(vnav_text[vnav]);
106     setprop("instrumentation/flightdirector/vertical-mode",vMode[vnav]);
107 },0,0);
108
109 setlistener("/instrumentation/flightdirector/spd", func(sp){
110     spd = sp.getValue();
111     if(spd == 0){AP_spd.setValue("");}
112     if(spd == 1){AP_spd.setValue("speed-with-throttle");}
113 },0,0);
114
115 setlistener("/instrumentation/nav/slaved-to-gps", func(slv){
116     slaved = slv.getBoolValue();
117 },0,0);
118
119 setlistener("/instrumentation/nav/radials/selected-deg", func(rd){
120     course = rd.getValue();
121     if(course == nil){course=0.0;}
122     course += mag_offset;
123     if(course >360){course -= 360;}
124     props.globals.getNode("instrumentation/gps/wp/wp[0]/desired-course-deg").setValue(course);
125     props.globals.getNode("instrumentation/gps/wp/wp[1]/desired-course-deg").setValue(course);
126 },0,0);
127
128 setlistener("/instrumentation/primus1000/dc550/fms", func(fms){
129     var test =getprop("/instrumentation/flightdirector/lnav");
130     var test1 =getprop("/instrumentation/flightdirector/vnav");
131     if(fms.getBoolValue()){
132         if(test ==2 or test==3){test=4;}
133         if(test1 ==2){test1=1;}
134     }else{
135         if(test ==4){test=2;}
136         if(test1 ==1){test1=2;}
137     }
138     setprop("/instrumentation/flightdirector/lnav",test);
139     setprop("/instrumentation/flightdirector/vnav",test1);
140 },0,0);
141
142 var handle_inputs = func {
143     var nm = 0.0;
144     var hdg = 0.0;
145     var nav_brg=0.0;
146     var ap_hdg=0;
147     var track =0;
148     track =props.globals.getNode("/orientation/heading-deg").getValue();
149     maxroll = props.globals.getNode("/orientation/roll-deg",1).getValue();
150     if(maxroll > 45 or maxroll < -45){AP_passive.setBoolValue(1);}
151     maxpitch = props.globals.getNode("/orientation/pitch-deg").getValue();
152     if(maxpitch > 45 or maxpitch < -45){AP_passive.setBoolValue(1);}
153     if(props.globals.getNode("/position/altitude-agl-ft").getValue() < 100){AP_passive.setBoolValue(1);}
154     if(WP1.getNode("ID").getValue()!=nil){
155             nm = WP1.getNode("course-error-nm").getValue();
156             if(nm > 10){nm=10;}
157             if(nm < -10){nm=-10;}
158         }
159     GPS_CDI.setValue(nm);
160     hdg =RADIAL.getValue() + mag_offset;
161     hdg-=track;
162     if(hdg > 180){hdg-=360;}
163     if(hdg < -180){hdg+=360;}
164     if(slaved){
165         FD_deflection.setValue(nm);
166         nav_brg= WP1.getNode("bearing-true-deg").getValue();
167         if(nav_brg == nil){nav_brg = 0.0;}
168         }else{
169             FD_deflection.setValue(HDG_deflection.getValue());
170             nav_brg= props.globals.getNode("instrumentation/nav/heading-deg").getValue();
171             if(nav_brg == nil){nav_brg = 0.0;}
172             nav_brg+= mag_offset;
173         }
174     nav_brg -=track;
175     if(nav_brg > 180){nav_brg -=360;}
176     if(nav_brg < -180){nav_brg +=360;}
177     NAV_BRG.setValue(nav_brg);
178     ap_hdg = hdg +(FD_deflection.getValue() *4);
179     if(ap_hdg > 180){ap_hdg -= 360;}
180     if(ap_hdg < -180){ap_hdg += 360;}
181     FD_heading.setValue(ap_hdg);
182 }
183
184 ####    update nav gps or nav setting    ####
185
186 var update = func {
187 if(GO==0){return;}
188     handle_inputs();
189     settimer(update, 0); 
190     }