finished off the annunciator panel ...
[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 lp = aircraft.lowpass.new(0.5);
21
22 lnav_text=["wing-leveler","dg-heading-hold","nav1-hold","nav1-hold","true-heading-hold","nav1-hold","nav1-hold"];
23 vnav_text=["pitch-hold","altitude-hold","altitude-hold","pitch-hold","gs1-hold","pitch-hold","vertical-speed-hold","altitude-hold"];
24 var mag_offset=0;
25 var lMode=[" ","HDG","VOR","LOC","LNAV","VAPP","BC"];
26 var vMode=[" ","ALT","ASEL","GA","GS","VNAV","VS","FLC"];
27 var FMS = props.globals.getNode("/instrumentation/primus1000/dc550",1);
28 AP_hdg = props.globals.getNode("/autopilot/locks/heading",1);
29 AP_alt = props.globals.getNode("/autopilot/locks/altitude",1);
30 AP_spd = props.globals.getNode("/autopilot/locks/speed",1);
31 AP_lnav = props.globals.getNode("/instrumentation/flightdirector/lnav",1);
32 FD_deflection = props.globals.getNode("/instrumentation/flightdirector/crs-deflection",1);
33 FD_heading = props.globals.getNode("/instrumentation/flightdirector/hdg-deg",1);
34 FD_pitch = props.globals.getNode("/instrumentation/flightdirector/Pitch",1);
35 FD_roll = props.globals.getNode("/instrumentation/flightdirector/Roll",1);
36 HDG_deflection = props.globals.getNode("/instrumentation/nav/heading-needle-deflection",1);
37 AP_vnav = props.globals.getNode("/instrumentation/flightdirector/vnav",1);
38 AP_speed = props.globals.getNode("/instrumentation/flightdirector/spd",1);
39 AP_passive = props.globals.getNode("/autopilot/locks/passive-mode",1);
40 BC_btn = props.globals.getNode("/instrumentation/nav/back-course-btn",1); 
41 GPS_on = props.globals.getNode("/instrumentation/gps/serviceable",1);
42 WP1 = props.globals.getNode("/instrumentation/gps/wp/wp[1]",1);
43 RADIAL =props.globals.getNode("/instrumentation/nav/radials/selected-deg",1);
44 NAV_BRG = props.globals.getNode("/instrumentation/flightdirector/nav-mag-brg",1);
45
46 setlistener("/sim/signals/fdm-initialized", func {
47     current_alt = props.globals.getNode("/instrumentation/altimeter/indicated-altitude-ft").getValue();
48     alt_select = props.globals.getNode("/autopilot/settings/target-altitude-ft").getValue();
49     AP_speed.setValue(0);
50     AP_lnav.setValue(0);
51     AP_vnav.setValue(0);
52     AP_passive.setBoolValue(1);
53     BC_btn.setValue(0);
54     course = props.globals.getNode("/instrumentation/nav/radials/selected-deg").getValue();
55     slaved = props.globals.getNode("/instrumentation/nav/slaved-to-gps").getBoolValue();
56     props.globals.getNode("instrumentation/gps/wp/wp[0]/desired-course-deg").setValue(course);
57     props.globals.getNode("instrumentation/gps/wp/wp[1]/desired-course-deg").setValue(course);
58     GPS_on.setBoolValue(1);
59     mag_offset=props.globals.getNode("/environment/magnetic-variation-deg").getValue();
60     GO = 1;
61     settimer(update, 2);
62     print("Flight Director ...Check");
63 });
64
65 ####    Mode Controller inputs    ####
66
67 setlistener("/instrumentation/flightdirector/lnav", func(ln){
68     lnav = ln.getValue();
69     var Vn=getprop("/instrumentation/flightdirector/vnav");
70     if(Vn==nil){Vn=0;}
71     if(lnav == 0 or lnav ==nil){
72         BC_btn.setBoolValue(0);
73     }
74     if(lnav == 1){
75         BC_btn.setBoolValue(0);
76         if(Vn ==4 ){Vn = 0;}
77     }
78     if(lnav == 2){
79         if(getprop("/instrumentation/primus1000/dc550/fms")){
80         lnav=4;
81         }
82     }
83     if(lnav == 3){BC_btn.setBoolValue(0);
84         if(!getprop("instrumentation/nav/nav-loc")){
85             lnav=2;
86         }
87     }
88     if(lnav == 5){BC_btn.setBoolValue(0);
89         if(getprop("instrumentation/nav/has-gs")){
90             Vn=4;
91         }
92     }
93     if(lnav == 6){BC_btn.setBoolValue(1);
94         if(Vn==4){Vn = 0;}
95     }
96     AP_hdg.setValue(lnav_text[lnav]);
97     setprop("instrumentation/flightdirector/lateral-mode",lMode[lnav]);
98     setprop("instrumentation/flightdirector/vnav",Vn);
99 },0,0);
100
101 setlistener("/instrumentation/flightdirector/vnav", func(vn){
102     vnav = vn.getValue();
103     if(vnav == 4){
104         if (!getprop("/instrumentation/nav/has-gs",1)){
105             vnav = 0;
106         }
107     }
108     if(vnav == 5){
109         if (!getprop("/instrumentation/nav/has-gs",1)){
110             vnav = 0;
111         }else{
112         setprop("/autopilot/settings/target-pitch-deg",0);
113         }
114     }
115     AP_alt.setValue(vnav_text[vnav]);
116     setprop("instrumentation/flightdirector/vertical-mode",vMode[vnav]);
117 },0,0);
118
119 setlistener("/instrumentation/flightdirector/spd", func(sp){
120     spd = sp.getValue();
121     if(spd == 0){AP_spd.setValue("");}
122     if(spd == 1){AP_spd.setValue("speed-with-throttle");}
123 },0,0);
124
125 setlistener("/instrumentation/nav/radials/selected-deg", func(rd){
126     course = rd.getValue();
127     if(course == nil){course=0.0;}
128     course += mag_offset;
129     if(course >360){course -= 360;}
130     props.globals.getNode("instrumentation/gps/wp/wp[0]/desired-course-deg").setValue(course);
131     props.globals.getNode("instrumentation/gps/wp/wp[1]/desired-course-deg").setValue(course);
132 },0,0);
133
134 setlistener("/instrumentation/primus1000/dc550/fms", func(fms){
135     var test =getprop("/instrumentation/flightdirector/lnav");
136     var test1 =getprop("/instrumentation/flightdirector/vnav");
137     if(fms.getBoolValue()){
138         if(test ==2 or test==3){test=4;}
139         if(test1 ==2){test1=1;}
140     }else{
141         if(test ==4){test=2;}
142         if(test1 ==1){test1=2;}
143     }
144     setprop("/instrumentation/flightdirector/lnav",test);
145     setprop("/instrumentation/flightdirector/vnav",test1);
146 },0,0);
147
148 var handle_inputs = func {
149       
150     var maxroll = getprop("/orientation/roll-deg");
151     var maxpitch = getprop("/orientation/pitch-deg");
152
153     if(maxroll > 45 or maxroll < -45)AP_passive.setBoolValue(1);
154     if(maxpitch > 45 or maxpitch < -45)AP_passive.setBoolValue(1);
155     if(getprop("/position/altitude-agl-ft") < 100)AP_passive.setBoolValue(1);
156
157
158
159 }
160
161 ####    update nav gps or nav setting    ####
162
163 var update = func {
164 if(GO==0){return;}
165     handle_inputs();
166     settimer(update, 0); 
167     }