Mostly flightdirector updates ....
[fg:toms-fgdata.git] / Aircraft / Citation-Bravo / Nasal / flightdirector.nas
1 #############################################################################
2 # Flight Director/Autopilot controller.
3 # Syd Adams
4 # HDG:
5 # Heading Bug hold - Low Bank can be selected
6 #
7 # NAV:
8 # Arm & Capture VOR , LOC or FMS
9 #
10 # APR : (ILS approach)
11 # Arm & Capture VOR APR , LOC or BC
12 # Also arm and capture GS
13 #
14 # BC :
15 # Arm & capture localizer backcourse
16 # Nav also illuminates
17 #
18 # VNAV:
19 # Arm and capture VOR/DME or FMS vertical profile
20 # profile entered in MFD VNAV menu
21 #
22 # ALT:
23 # Hold current Altitude or PFD preset altitude
24 #
25 # VS:
26 # Hold current vertical speed
27 # adjustable with pitch wheel
28 #
29 # SPD :
30 # Hold current speed 
31 # adjustable with pitch wheel
32 #
33 #############################################################################
34
35 # lnav 
36 #0=W-LVL
37 #1=HDG,
38 #2=VOR Arm
39 #3=VOR Cap
40 #4=LOC Arm
41 #5=LOC Cap
42 #6=FMS Cap
43 #7=APR Arm
44 #8=BC Arm
45 #9=BC Cap
46
47 # vnav
48 #- 0=PIT
49 # 1=VNAV Arm,
50 #2=VNAV Cap,
51 #3=ALT hold
52 #4=ASEL
53 #5=VS
54 #6=GS Arm
55 #7=GS Cap
56
57
58 var lnav_text=["wing-leveler","dg-heading-hold","dg-heading-hold","nav1-hold","dg-heading-hold","nav1-hold","true-heading-hold","","dg-heading-hold","nav1-hold"];
59 var vnav_text=["pitch-hold","vs-hold","vs-hold","altitude-hold","vertical-speed-hold","vertical-speed-hold","","gs1-hold"];
60
61 var FMS = 0;
62 var lnav=0;
63 var vnav=0;
64 var in_range=0;
65 var Defl = props.globals.getNode("/instrumentation/nav/heading-needle-deflection");
66 var GSDefl = props.globals.getNode("/instrumentation/nav/gs-needle-deflection");
67 var AP_hdg = props.globals.getNode("/autopilot/locks/heading",1);
68 var AP_alt = props.globals.getNode("/autopilot/locks/altitude",1);
69 var AP_spd = props.globals.getNode("/autopilot/locks/speed",1);
70 var FD_lnav = props.globals.getNode("/instrumentation/flightdirector/lnav",1);
71 FD_lnav.setIntValue(0);
72 var FD_vnav = props.globals.getNode("/instrumentation/flightdirector/vnav",1);
73 FD_vnav.setIntValue(0);
74 var FD_pitch = props.globals.getNode("/instrumentation/flightdirector/Pitch",1);
75 var FD_roll = props.globals.getNode("/instrumentation/flightdirector/Roll",1);
76 var FD_asel = props.globals.getNode("/instrumentation/flightdirector/Asel",1);
77 var FD_speed = props.globals.getNode("/instrumentation/flightdirector/spd",1);
78 var DH = props.globals.getNode("/autopilot/route-manager/min-lock-altitude-agl-ft",1);
79
80
81 setlistener("/sim/signals/fdm-initialized", func {
82     AP_spd.setValue("");
83     AP_hdg.setValue(lnav_text[0]);
84     AP_alt.setValue(vnav_text[0]);
85     setprop("/autopilot/locks/passive-mode",1);
86     setprop("autopilot/settings/target-altitude-ft",0);
87     props.globals.getNode("instrumentation/primus1000/dc550/fms",1).setBoolValue(0);
88     FMS=0;
89     settimer(update, 5);
90     print("Flight Director ...Check");
91 });
92
93 ####    FD Controller inputs    ####
94 #### LATERAL MODE####
95 var set_lateral_mode=func{
96     if(lnav==2){
97         if(getprop("/instrumentation/nav/nav-loc")!=0)lnav=4;
98         if(FMS==1)lnav = 6;
99         }
100     if(lnav==7){
101     if(getprop("/instrumentation/nav/nav-loc")!=0){
102         if(getprop("instrumentation/nav/has-gs"))FD_vnav.setValue(6);
103         lnav=4;
104         }else{
105         lnav=2;
106         }
107     }
108     FD_lnav.setValue(lnav);
109     AP_hdg.setValue(lnav_text[lnav]);
110 }
111
112 #### VERTICAL MODE####
113 var set_vertical_mode=func{
114 setprop("autopilot/settings/vertical-speed-fpm",getprop("velocities/vertical-speed-fps") * 60);
115 setprop("autopilot/settings/target-pitch-deg",getprop("orientation/pitch-deg"));
116 if(vnav==3){
117 if(getprop("autopilot/settings/target-altitude-ft") < DH.getValue())vnav=0;
118     }
119 if(vnav==6){
120 vnav_text[6]=getprop("autopilot/locks/altitude");
121     }
122 FD_vnav.setValue(vnav);
123 AP_alt.setValue(vnav_text[vnav]);
124 }
125
126 setlistener("/instrumentation/primus1000/dc550/fms", func(fms){
127     if(fms.getBoolValue()){
128         FMS=1;
129         }else{
130         FMS=0;
131         FD_lnav.setValue(0);
132         FD_vnav.setValue(0);
133         }
134 },0,0);
135
136 setlistener("/instrumentation/flightdirector/lnav", func(ln){
137 lnav=ln.getValue();
138 set_lateral_mode();
139 },0,0);
140
141 setlistener("/instrumentation/flightdirector/vnav", func(vn){
142 vnav=vn.getValue();
143 set_vertical_mode();
144 },0,0);
145
146
147 ####    update nav gps or nav setting    ####
148
149 var update = func {
150
151 var dst = getprop("instrumentation/nav/nav-distance");
152     if(dst == nil or dst >30000){
153     in_range=0;
154     }else{
155     in_range=1;
156     }
157     
158     var APoff = getprop("/autopilot/locks/passive-mode");
159     if(APoff == 0){
160     var maxroll = getprop("/orientation/roll-deg");
161     var maxpitch = getprop("/orientation/pitch-deg");
162     if(maxroll > 45 or maxroll < -45)APoff = 1;
163     if(maxpitch > 45 or maxpitch < -45)APoff = 1;
164     if(getprop("/position/altitude-agl-ft") < DH.getValue())APoff = 1;
165     setprop("/autopilot/locks/passive-mode", APoff);
166 }
167
168
169 if(FMS==0){
170     var deflection = Defl.getValue();
171     var gs_deflection = GSDefl.getValue();
172     var capture = 0;
173     
174     if(lnav ==2 or lnav==4){
175         if(deflection > -7 or deflection < 7){
176             if(in_range==1){
177                 lnav+=1;
178                 }
179             }
180         FD_lnav.setValue(lnav);
181     }
182
183     if(lnav ==5){
184         if(vnav==6){
185             if(gs_deflection  < 1.0 and gs_deflection > -1.0){
186                 if(in_range==1)vnav = 7;
187                 FD_vnav.setValue(vnav);
188                 }
189             }
190         }
191     }
192
193
194 if(vnav == 4){
195 var TGALT = getprop("autopilot/settings/target-altitude-ft");
196     if (TGALT > DH.getValue()){
197     var MyAlt = getprop("position/altitude-ft");
198         if(MyAlt > (TGALT -1000) or MyAlt < (TGALT +1000)){
199             vnav=3;
200             }
201         }else{
202         vnav=0;
203         }
204     AP_alt.setValue(vnav_text[vnav]);
205     FD_vnav.setValue(vnav);
206     }
207
208
209 settimer(update, 0); 
210 }