Remove old references to /sim/auto-coordination.
[fg:toms-fgdata.git] / Aircraft / CitationX / Nasal / flightdirector.nas
1 #############################################################################
2 # Flight Director/Autopilot controller.
3 # Syd Adams
4 #
5 ###  Initialization #######
6
7 var Lateral = "autopilot/locks/heading";
8 var Lateral_arm = "autopilot/locks/heading-arm";
9 var Vertical = "autopilot/locks/altitude";
10 var Vertical_arm = "autopilot/locks/altitude-arm";
11 var AP = "autopilot/locks/AP-status";
12 var NAVprop="autopilot/settings/nav-source";
13 var AutoCoord="controls/flight/auto-coordination";
14 var NAVSRC= getprop(NAVprop);
15 var count=0;
16 var Coord = 0;
17 var minimums=getprop("autopilot/settings/minimums");
18 var wx_range=[10,25,50,100,200,300];
19 var wx_index=3;
20
21
22 #####################################
23
24 setlistener("/sim/signals/fdm-initialized", func {
25     setprop("instrumentation/nd/range",wx_range[wx_index]);
26     print("Flight Director ...Check");
27     settimer(update_fd, 30);
28 });
29
30 ### AP /FD BUTTONS ####
31
32 var FD_set_mode = func(btn){
33     var Lmode=getprop(Lateral);
34     var LAmode=getprop(Lateral_arm);
35     var Vmode=getprop(Vertical);
36     var VAmode=getprop(Vertical_arm);
37
38
39     if(btn=="ap"){
40         Coord = getprop(AutoCoord);
41         if(getprop(AP)!="AP1"){
42             setprop(Lateral_arm,"");setprop(Vertical_arm,"");
43             if(Vmode=="PTCH")set_pitch();
44             if(Lmode=="ROLL")set_roll();
45             if(getprop("position/altitude-agl-ft") > minimums) {
46                 setprop(AP,"AP1");setprop(AutoCoord,0);}
47         } else kill_Ap("");
48     }elsif(btn=="hdg"){
49         if(Lmode!="HDG") setprop(Lateral,"HDG") else set_roll();
50         setprop(Lateral_arm,"");setprop(Vertical_arm,"");
51     }elsif(btn=="alt"){
52         setprop(Lateral_arm,"");setprop(Vertical_arm,"");
53         if(Vmode!="ALT"){
54             setprop(Vertical,"ALT");setprop("autopilot/settings/asel",(getprop("instrumentation/altimeter/mode-c-alt-ft") * 0.01));
55         } else set_pitch();
56     }elsif(btn=="flc"){
57         var flcmode = "FLC";
58         var asel = "ASEL";
59         if(NAVSRC=="FMS"){flcmode="VFLC";asel = "VASEL";}
60         if(Vmode!=flcmode){
61             var mc =getprop("instrumentation/airspeed-indicator/indicated-mach");
62             var kt=int(getprop("instrumentation/airspeed-indicator/indicated-speed-kt"));
63             if(!getprop("autopilot/settings/changeover")){
64                 if(kt > 80 and kt <340){
65                     setprop(Vertical,flcmode);
66                     setprop(Vertical_arm,asel);
67                     setprop("autopilot/settings/target-speed-kt",kt);
68                     setprop("autopilot/settings/target-speed-mach",mc);
69                 }
70             }else{
71                 if(mc > 0.40 and mc <0.85){
72                     setprop(Vertical,flcmode);
73                     setprop(Vertical_arm,asel);
74                     setprop("autopilot/settings/target-speed-kt",kt);
75                     setprop("autopilot/settings/target-speed-mach",mc);
76                 }
77             }
78         } else set_pitch();
79     }elsif(btn=="nav"){
80         set_nav_mode();
81         setprop("autopilot/settings/low-bank",0);
82     }elsif(btn=="vnav"){
83         if(Vmode!="VALT"){
84             if(NAVSRC=="FMS"){
85                 setprop(Vertical,"VALT");
86                 setprop(Lateral,"LNAV");
87             }
88             }else set_pitch();
89     }elsif(btn=="app"){
90         setprop(Lateral_arm,"");setprop(Vertical_arm,"");
91         set_apr();
92         setprop("autopilot/settings/low-bank",0);
93     }elsif(btn=="vs"){
94         setprop(Lateral_arm,"");setprop(Vertical_arm,"");
95          if(Vmode!="VS"){
96             var tgt_vs = (int(getprop("autopilot/internal/vert-speed-fpm") * 0.01)) * 100;
97             setprop(Vertical,"VS");setprop("autopilot/settings/vertical-speed-fpm",tgt_vs);
98         } else set_pitch();
99     }elsif(btn=="stby"){
100         setprop(Lateral_arm,"");setprop(Vertical_arm,"");
101         set_pitch();
102         set_roll();
103         setprop("autopilot/settings/low-bank",0);
104     }elsif(btn=="bank"){
105         var Bnk="autopilot/settings/low-bank";
106         if(Lmode=="HDG")setprop(Bnk,1-getprop(Bnk));
107     }elsif(btn=="co"){
108         var Co= 1- getprop("autopilot/settings/changeover");
109          if(Vmode!="FLC") Co=0;
110          setprop("autopilot/settings/changeover",Co);
111     }
112 }
113
114 ########  FMS/NAV BUTTONS  ############
115 var nav_src_set=func(src){
116     setprop(Lateral_arm,"");setprop(Vertical_arm,"");
117     set_pitch();set_roll();
118     if(src=="fms"){
119         if(getprop("autopilot/route-manager/route/num")>0)setprop(NAVprop,"FMS");
120     }else{
121         if (NAVSRC!="NAV1")setprop(NAVprop,"NAV1") else setprop(NAVprop,"NAV2");
122     }
123 }
124
125 ########### ARM VALID NAV MODE ################
126
127 var set_nav_mode=func{
128     setprop(Lateral_arm,"");setprop(Vertical_arm,"");
129     if(NAVSRC=="NAV1"){
130         if(getprop("instrumentation/nav/data-is-valid")){
131             if(getprop("instrumentation/nav/nav-loc"))setprop(Lateral_arm,"LOC") else setprop(Lateral_arm,"VOR");
132             setprop(Lateral,"HDG");
133         }
134     }elsif(NAVSRC=="NAV2"){
135         if(getprop("instrumentation/nav[1]/data-is-valid")){
136             if(getprop("instrumentation/nav[1]/nav-loc"))setprop(Lateral_arm,"LOC") else setprop(Lateral_arm,"VOR");
137             setprop(Lateral,"HDG");
138         }
139     }elsif(NAVSRC=="FMS"){
140         if(getprop("autopilot/route-manager/active"))setprop(Lateral,"LNAV");
141     }
142 }
143
144 #######  PITCH WHEEL ACTIONS #############
145
146 var pitch_wheel=func(dir){
147     var Vmode=getprop(Vertical);
148     var CO = getprop("autopilot/settings/changeover");
149     var amt=0;
150     if(Vmode=="VS"){
151         amt = int(getprop("autopilot/settings/vertical-speed-fpm")) + (dir* 100);
152         amt = (amt < -8000 ? -8000 : amt > 6000 ? 6000 : amt);
153         setprop("autopilot/settings/vertical-speed-fpm",amt);
154     }elsif(Vmode=="FLC" or Vmode=="VFLC"){
155         if(!CO){
156             amt=getprop("autopilot/settings/target-speed-kt") + dir;
157             amt = (amt < 80 ? 80 : amt > 340 ? 340 : amt);
158             setprop("autopilot/settings/target-speed-kt",amt);
159         }else{
160             amt=getprop("autopilot/settings/target-speed-mach") + (dir*0.01);
161             amt = (amt < 0.40 ? 0.40 : amt > 0.85 ? 0.85 : amt);
162             setprop("autopilot/settings/target-speed-mach",amt);
163         }
164     }elsif(Vmode=="PTCH"){
165         amt=getprop("autopilot/settings/target-pitch-deg") + (dir*0.1);
166         amt = (amt < -20 ? -20 : amt > 20 ? 20 : amt);
167         setprop("autopilot/settings/target-pitch-deg",amt)
168     }
169 }
170
171 ########    FD INTERNAL ACTIONS  #############
172
173 var set_pitch=func{
174     setprop(Vertical,"PTCH");setprop("autopilot/settings/target-pitch-deg",getprop("orientation/pitch-deg"));
175 }
176
177 var set_roll=func{
178     setprop(Lateral,"ROLL");setprop("autopilot/settings/target-roll-deg",0.0);
179 }
180
181 var set_apr=func{
182     if(NAVSRC == "NAV1"){
183         if(getprop("instrumentation/nav/nav-loc") and getprop("instrumentation/nav/has-gs")){
184             setprop(Lateral_arm,"LOC");
185             setprop(Vertical_arm,"GS");
186             setprop(Lateral,"HDG");
187         }
188     }elsif(NAVSRC == "NAV2"){
189     if(getprop("instrumentation/nav[1]/nav-loc") and getprop("instrumentation/nav[1]/has-gs")){
190             setprop(Lateral_arm,"LOC");
191             setprop(Vertical_arm,"GS");
192             setprop(Lateral,"HDG");
193         }
194     }
195 }
196
197 setlistener("autopilot/settings/minimums", func(mn) {
198     minimums=mn.getValue();
199 },1,0);
200
201
202 setlistener(NAVprop, func(Nv) {
203     NAVSRC=Nv.getValue();
204 },1,0);
205
206 var update_nav=func{
207     var sgnl = "- - -";
208     var gs =0;
209     if(NAVSRC == "NAV1"){
210         if(getprop("instrumentation/nav/data-is-valid"))sgnl="VOR1";
211         setprop("autopilot/internal/in-range",getprop("instrumentation/nav/in-range"));
212         setprop("autopilot/internal/gs-in-range",getprop("instrumentation/nav/gs-in-range"));
213         var dst=getprop("instrumentation/nav/nav-distance") or 0;
214         dst*=0.000539;
215         setprop("autopilot/internal/nav-distance",dst);
216         setprop("autopilot/internal/nav-id",getprop("instrumentation/nav/nav-id"));
217         if(getprop("instrumentation/nav/nav-loc"))sgnl="LOC1";
218         if(getprop("instrumentation/nav/has-gs"))sgnl="ILS1";
219         if(sgnl=="ILS1")gs = 1;
220         setprop("autopilot/internal/gs-valid",gs);
221         setprop("autopilot/internal/nav-type",sgnl);
222         course_offset("instrumentation/nav[0]/radials/selected-deg");
223         setprop("autopilot/internal/to-flag",getprop("instrumentation/nav/to-flag"));
224         setprop("autopilot/internal/from-flag",getprop("instrumentation/nav/from-flag"));
225     }elsif(NAVSRC == "NAV2"){
226         if(getprop("instrumentation/nav[1]/data-is-valid"))sgnl="VOR2";
227         setprop("autopilot/internal/in-range",getprop("instrumentation/nav[1]/in-range"));
228         setprop("autopilot/internal/gs-in-range",getprop("instrumentation/nav[1]/gs-in-range"));
229         var dst=getprop("instrumentation/nav[1]/nav-distance") or 0;
230         dst*=0.000539;
231         setprop("autopilot/internal/nav-distance",dst);
232         setprop("autopilot/internal/nav-id",getprop("instrumentation/nav[1]/nav-id"));
233         if(getprop("instrumentation/nav[1]/nav-loc"))sgnl="LOC2";
234         if(getprop("instrumentation/nav[1]/has-gs"))sgnl="ILS2";
235         if(sgnl=="ILS2")gs = 1;
236         setprop("autopilot/internal/gs-valid",gs);
237         setprop("autopilot/internal/nav-type",sgnl);
238         course_offset("instrumentation/nav[1]/radials/selected-deg");
239         setprop("autopilot/internal/to-flag",getprop("instrumentation/nav[1]/to-flag"));
240         setprop("autopilot/internal/from-flag",getprop("instrumentation/nav[1]/from-flag"));
241     }elsif(NAVSRC == "FMS"){
242         setprop("autopilot/internal/nav-type","FMS1");
243         setprop("autopilot/internal/in-range",1);
244         setprop("autopilot/internal/gs-in-range",0);
245         setprop("autopilot/internal/nav-distance",getprop("instrumentation/gps/wp/wp[1]/distance-nm"));
246         setprop("autopilot/internal/nav-id",getprop("instrumentation/gps/wp/wp[1]/ID"));
247         course_offset("instrumentation/gps/wp/wp[1]/bearing-mag-deg");
248         setprop("autopilot/internal/to-flag",getprop("instrumentation/gps/wp/wp[1]/to-flag"));
249         setprop("autopilot/internal/from-flag",getprop("instrumentation/gps/wp/wp[1]/from-flag"));
250     }
251 }
252
253 var set_range = func(dir){
254     wx_index+=dir;
255     if(wx_index>5)wx_index=5;
256     if(wx_index<0)wx_index=0;
257     setprop("instrumentation/nd/range",wx_range[wx_index]);
258 }
259
260 var course_offset = func(src){
261     var crs_set=getprop(src);
262     var crs_offset= crs_set - getprop("orientation/heading-magnetic-deg");
263     if(crs_offset>180)crs_offset-=360;
264     if(crs_offset<-180)crs_offset+=360;
265     setprop("autopilot/internal/course-offset",crs_offset);
266     crs_offset+=getprop("autopilot/internal/cdi");
267     if(crs_offset>180)crs_offset-=360;
268     if(crs_offset<-180)crs_offset+=360;
269     setprop("autopilot/internal/ap_crs",crs_offset);
270     setprop("autopilot/internal/selected-crs",crs_set);
271 }
272
273 var monitor_L_armed = func{
274     if(getprop(Lateral_arm)!=""){
275         if(getprop("autopilot/internal/in-range")){
276             var cdi=getprop("autopilot/internal/cdi");
277             if(cdi < 40 and cdi > -40){
278                 setprop(Lateral,getprop(Lateral_arm));
279                 setprop(Lateral_arm,"");
280             }
281         }
282     }
283 }
284
285 var monitor_V_armed = func{
286     var Varm=getprop(Vertical_arm);
287     var myalt=getprop("instrumentation/altimeter/indicated-altitude-ft");
288     var asel=getprop("autopilot/settings/asel") * 100;
289     var alterr=myalt-asel;
290     if(Varm=="ASEL"){
291         if(alterr >-250 and alterr <250){
292             setprop(Vertical,"ALT");
293             setprop(Vertical_arm,"");
294         }
295     }elsif(Varm=="VASEL"){
296         if(alterr >-250 and alterr <250){
297             setprop(Vertical,"VALT");
298             setprop("instrumentation/gps/wp/wp[1]/altitude-ft",asel);
299             setprop(Vertical_arm,"");
300         }
301     }elsif(Varm=="GS"){
302         if(getprop(Lateral)=="LOC"){
303             if(getprop("autopilot/internal/gs-in-range")){
304                 var gs_err=getprop("autopilot/internal/gs-deflection");
305                 var gs_dst=getprop("autopilot/internal/nav-distance");
306                 if(gs_dst <= 7.0){
307                     if(gs_err >-0.25 and gs_err < 0.25){
308                         setprop(Vertical,"GS");
309                         setprop(Vertical_arm,"");
310                     }
311                 }
312             }
313         }
314     }
315 }
316
317 var monitor_AP_errors= func{
318     var ralt=getprop("position/altitude-agl-ft");
319     if(ralt<minimums)kill_Ap("");
320     var rlimit=getprop("orientation/roll-deg");
321     if(rlimit > 45 or rlimit< -45)kill_Ap("AP-FAIL");
322     var plimit=getprop("orientation/pitch-deg");
323     if(plimit > 30 or plimit< -30)kill_Ap("AP-FAIL");
324 }
325
326 var kill_Ap = func(msg){
327     setprop(AP,msg);
328     setprop(AutoCoord,Coord);
329 }
330
331 var get_ETE= func{
332     var ttw = "--:--";
333     var min =0;
334     var hr=0;
335     if(NAVSRC == "NAV1"){
336         setprop("instrumentation/dme/frequencies/source","instrumentation/nav/frequencies/selected-mhz");
337         min = int(getprop("instrumentation/dme/indicated-time-min"));
338         if(min>60){
339             var tmphr=(min*0.016666);
340             hr=int(tmphr);
341             var tmpmin=(tmphr-hr)*100;
342             min=int(tmpmin);
343         }
344         ttw=sprintf("ETE %i:%02i",hr,min);
345     }elsif(NAVSRC == "NAV2"){
346         setprop("instrumentation/dme/frequencies/source","instrumentation/nav[1]/frequencies/selected-mhz");
347         min = int(getprop("instrumentation/dme/indicated-time-min"));
348         if(min>60){
349             var tmphr=(min*0.016666);
350             hr=int(tmphr);
351             var tmpmin=(tmphr-hr)*100;
352             min=int(tmpmin);
353         }
354         ttw=sprintf("ETE %s:%02i",hr,min);
355     }elsif(NAVSRC == "FMS"){
356         min = getprop("autopilot/route-manager/ete");
357         min=int(min * 0.016666);
358         if(min>60){
359             var tmphr=(min*0.016666);
360             hr=int(tmphr);
361             var tmpmin=(tmphr-hr)*100;
362             min=int(tmpmin);
363         }
364          ttw=sprintf("ETE %s:%02i",hr,min);
365     }
366     setprop("autopilot/internal/nav-ttw",ttw);
367 }
368
369
370 ###  Main loop ###
371
372 var update_fd = func {
373     update_nav();
374     if(count==0)monitor_AP_errors();
375     if(count==1)monitor_L_armed();
376     if(count==2)monitor_V_armed();
377      if(count==3)get_ETE();
378     count+=1;
379     if(count>3)count=0;
380     settimer(update_fd, 0);
381 }