Get author's latest changes and add Lembrandt landing light
[fg:toms-fgdata.git] / Aircraft / PC-9M / Nasal / AFDS.nas
1 #############################################################################
2 # PC-9M Autopilot Flight Director System
3 # Hyde Yamakawa
4 #
5 # speed modes: SPD;
6 # roll modes : HDG SEL,HDG HOLD, LNAV, LOC;
7 # pitch modes: ALT, V/S, G/S;
8 # FPA range  : -9.9 ~ 9.9 degrees
9 # VS range   : -8000 ~ 6000
10 # ALT range  : 0 ~ 50,000
11 # KIAS range : 100 ~ 399
12 # MACH range : 0.40 ~ 0.95
13 #
14 #############################################################################
15
16 #Usage : var afds = AFDS.new();
17
18 var copilot = func(msg) { setprop("/sim/messages/copilot",msg);}
19
20 var AFDS = 
21 {
22         new : func
23         {
24                 var m = {parents:[AFDS]};
25
26                 m.spd_list=["","SPD"];
27
28                 m.roll_list=["","HDG HOLD","LNAV","LOC"];
29
30                 m.pitch_list=["","ALT","V/S","G/S"];
31
32                 m.step=0;
33                 m.remaining_distance_log_last = 36000;
34                 m.heading_change_rate = 0.4 * 0.7;
35
36                 m.AFDS_node = props.globals.getNode("instrumentation/afds",1);
37                 m.AFDS_inputs = m.AFDS_node.getNode("inputs",1);
38                 m.AFDS_apmodes = m.AFDS_node.getNode("ap-modes",1);
39                 m.AFDS_settings = m.AFDS_node.getNode("settings",1);
40                 m.AP_settings = props.globals.getNode("autopilot/settings",1);
41
42                 m.AP = m.AFDS_inputs.initNode("AP",0,"BOOL");
43                 m.AP_disengaged = m.AFDS_inputs.initNode("AP-disengage",0,"BOOL");
44                 m.AP_passive = props.globals.initNode("autopilot/locks/passive-mode",1,"BOOL");
45                 m.AP_pitch_engaged = props.globals.initNode("autopilot/locks/pitch-engaged",1,"BOOL");
46                 m.AP_roll_engaged = props.globals.initNode("autopilot/locks/roll-engaged",1,"BOOL");
47
48                 m.FD = m.AFDS_inputs.initNode("FD",0,"BOOL");
49                 m.at1 = m.AFDS_inputs.initNode("at-armed",0,"BOOL");
50                 m.autothrottle_mode = m.AFDS_inputs.initNode("autothrottle-index",0,"INT");
51                 m.lateral_mode = m.AFDS_inputs.initNode("lateral-index",0,"INT");
52                 m.vertical_mode = m.AFDS_inputs.initNode("vertical-index",0,"INT");
53                 m.gs_armed = m.AFDS_inputs.initNode("gs-armed",0,"BOOL");
54                 m.loc_armed = m.AFDS_inputs.initNode("loc-armed",0,"BOOL");
55                 m.vor_armed = m.AFDS_inputs.initNode("vor-armed",0,"BOOL");
56                 m.lnav_armed = m.AFDS_inputs.initNode("lnav-armed",0,"BOOL");
57                 m.estimated_time_arrival = m.AFDS_inputs.initNode("estimated-time-arrival",0,"INT");
58                 m.remaining_distance = m.AFDS_inputs.initNode("remaining-distance",0,"DOUBLE");;
59         m.vs_ind =  props.globals.initNode("autopilot/internal/vert-speed-fpm",0,"DOUBLE");
60
61                 m.heading_magnetic = m.AFDS_settings.getNode("heading-magnetic",1);
62
63                 m.AP_roll_mode = m.AFDS_apmodes.initNode("roll-mode","");
64                 m.AP_pitch_mode = m.AFDS_apmodes.initNode("pitch-mode","");
65                 m.AP_speed_mode = m.AFDS_apmodes.initNode("speed-mode","");
66
67                 m.ias_setting = m.AP_settings.initNode("target-speed-kt",150);# 100 - 399 #
68                 m.vs_setting = m.AP_settings.initNode("vertical-speed-fpm",0); # -8000 to +6000 #
69                 m.hdg_setting = m.AP_settings.initNode("heading-bug-deg",360,"INT"); # 1 to 360
70                 m.alt_setting = m.AP_settings.initNode("counter-set-altitude-ft",5000,"DOUBLE");
71                 m.target_alt = m.AP_settings.initNode("actual-target-altitude-ft",5000,"DOUBLE");
72
73                 m.APl = setlistener(m.AP, func m.setAP(),0,0);
74                 m.APdisl = setlistener(m.AP_disengaged, func m.setAP(),0,0);
75                 return m;
76         },
77 ####  Inputs   ####
78 ###################
79         input : func(mode,btn)
80         {
81 #               if(getprop("/systems/electrical/outputs/avionics"))
82 #               {
83                         var current_alt = getprop("instrumentation/altimeter/indicated-altitude-ft");
84                         if(mode==0)
85                         {
86                                 # horizontal AP controls
87                                 if(btn == 1)            # Heading button
88                                 {
89                                         # set target to current magnetic heading
90                                         var tgtHdg = int(me.heading_magnetic.getValue() + 0.50);
91                                         me.hdg_setting.setValue(tgtHdg);
92                                 }
93                                 elsif(btn==2)           # LNAV button
94                                 {
95                                         if ((!getprop("/autopilot/route-manager/active"))or
96                                                 (getprop("/autopilot/route-manager/current-wp")<0)or
97                                                 (getprop("/autopilot/route-manager/wp/id")==""))
98                                         {
99                                                 # Oops, route manager isn't active. Keep current mode.
100                                                 btn = me.lateral_mode.getValue();
101                                                 copilot("Instractor:LNAV doesn't engage. We forgot to program or activate the route manager!");
102                                         }
103                                         else
104                                         {
105                                                 if(me.lateral_mode.getValue() == 2)             # Current mode is LNAV
106                                                 {
107                                                         # set target to current magnetic heading
108                                                         var tgtHdg = int(me.heading_magnetic.getValue() + 0.50);
109                                                         me.hdg_setting.setValue(tgtHdg);
110                                                         btn = 1;        # Heading sel
111                                                 }
112                                                 elsif(me.lnav_armed.getValue())
113                                                 {       # LNAV armed then disarm
114                                                         me.lnav_armed.setValue(0);
115                                                         btn = me.lateral_mode.getValue();
116                                                 }
117                                                 else
118                                                 {       # LNAV arm
119                                                         me.lnav_armed.setValue(1);
120                                                         btn = me.lateral_mode.getValue();
121                                                 }
122                                         }
123                                 }
124                                 me.lateral_mode.setValue(btn);
125                         }
126                         elsif(mode==1)
127                         {
128                                 # vertical AP controls
129                                 if (btn==1)
130                                 {
131                                         # hold current altitude
132                                         if(me.AP.getValue() or me.FD.getValue())
133                                         {
134                                                 var alt = int((current_alt+50)/100)*100;
135                                                 me.target_alt.setValue(alt);
136 #                                               me.autothrottle_mode.setValue(5);       # A/T SPD
137                                         }
138                                 }
139                                 if(btn==2)
140                                 {
141                                         # hold current vertical speed
142                                         var vs = me.vs_ind.getValue();
143                                         vs = int(vs/100)*100;
144                                         if (vs<-8000) vs = -8000;
145                                         if (vs>6000) vs = 6000;
146                                         me.vs_setting.setValue(vs);
147                                         me.target_alt.setValue(me.alt_setting.getValue());
148 #                                       me.autothrottle_mode.setValue(5);       # A/T SPD
149                                 }
150                                 me.vertical_mode.setValue(btn);
151                         }
152                         elsif(mode == 2)
153                         {
154                                 # throttle AP controls
155                                 if(me.autothrottle_mode.getValue() != 0
156                                         or (me.at1.getValue() == 0))
157                                 {
158                                         btn = 0;
159                                 }
160                                 elsif(getprop("position/altitude-agl-ft") > 400) # above baro 400 ft
161                                 {
162                                         btn=0;
163                                         copilot("Auto-throttle won't engage below 400ft.");
164                                 }
165                                 me.autothrottle_mode.setValue(btn);
166                         }
167                         elsif(mode==3)  #FD, LOC or G/S button
168                         {
169                                 var llocmode = me.lateral_mode.getValue();
170                                 if(btn==0)
171                                 {
172                                         if(llocmode == 3)               # Alrady in LOC mode
173                                         {
174                                                 # set target to current magnetic heading
175                                                 var tgtHdg = int(me.heading_magnetic.getValue() + 0.50);
176                                                 me.hdg_setting.setValue(tgtHdg);
177                                                 me.lateral_mode.setValue(2);            # Keep current headding
178                                                 me.loc_armed.setValue(0);                       # Disarm
179                                         }
180                                         elsif(me.loc_armed.getValue())                  # LOC armed but not captured yet
181                                         {
182                                                 me.loc_armed.setValue(0);                       # Disarm
183                                         }
184                                         else
185                                         {
186                                                 me.loc_armed.setValue(1);                       # LOC arm
187                                         }
188                                 }
189                                 elsif (btn==1)  #APP button
190                                 {
191                                         var lgsmode = me.vertical_mode.getValue();
192                                         if(lgsmode == 3)        # Already in G/S mode
193                                         {
194                                                 me.vertical_mode.setValue(1);   # Keep current altitude
195                                                 me.gs_armed.setValue(0);                # Disarm
196                                         }
197                                         elsif(me.gs_armed.getValue())           # G/S armed but not captured yet
198                                         {
199                                                 me.gs_armed.setValue(0);                # Disarm
200                                                 if(llocmode == 3)               # Alrady in LOC mode
201                                                 {
202                                                         # set target to current magnetic heading
203                                                         var tgtHdg = int(me.heading_magnetic.getValue() + 0.50);
204                                                         me.hdg_setting.setValue(tgtHdg);
205                                                         me.lateral_mode.setValue(1);            # Keep current headding
206                                                         me.loc_armed.setValue(0);                       # Disarm
207                                                 }
208                                                 else
209                                                 {
210                                                         me.loc_armed.setValue(0);                       # Disarm
211                                                 }
212                                         }
213                                         else
214                                         {
215                                                 me.gs_armed.setValue(1);                # G/S arm
216                                                 if(me.loc_armed.getValue() == 0)
217                                                 {
218                                                         me.loc_armed.setValue(1);               # LOC arm
219                                                 }
220                                         }
221                                 }
222                                 elsif(btn == 2) # FD button toggle
223                                 {
224                                         if(!me.FD.getValue())
225                                         {
226                                                 if(me.lateral_mode.getValue() == 0)             # Not set
227                                                 {
228                                                         me.lateral_mode.setValue(1);            # HDG HOLD
229                                                 }
230                                                 if(me.vertical_mode.getValue() == 0)    # Not set
231                                                 {
232                                                         var alt = int((current_alt+50)/100)*100;
233                                                         me.target_alt.setValue(alt);
234                                                         me.vertical_mode.setValue(1);           # ALT
235                                                 }
236                                                 me.FD.setValue(1)
237                                         }
238                                         else
239                                         {
240                                                 if(!me.AP.getValue())
241                                                 {
242                                                         me.lateral_mode.setValue(0);            # Clear
243                                                         me.vertical_mode.setValue(0);           # Clear
244                                                 }
245                                                 me.FD.setValue(0)
246                                         }
247                                 }
248                         }
249 #               }
250         },
251 ###################
252         setAP : func{
253                 var output = 1-me.AP.getValue();
254                 var disabled = me.AP_disengaged.getValue();
255                 if((output==0)and(getprop("position/altitude-agl-ft")<200))
256                 {
257                         disabled = 1;
258                         copilot("Instractor:Autopilot won't engage below 200ft.");
259                 }
260                 if((disabled)and(output==0)){output = 1;me.AP.setValue(0);}
261                 if (output==1)
262                 {
263                         var msg="";
264                         var msg2="";
265                         var msg3="";
266                         if (abs(getprop("controls/flight/rudder-trim"))   > 0.04) msg  = "rudder";
267                         if (abs(getprop("controls/flight/elevator-trim")) > 0.04) msg2 = "pitch";
268                         if (abs(getprop("controls/flight/aileron-trim"))  > 0.04) msg3 = "aileron";
269                         if (msg ~ msg2 ~ msg3 != "")
270                         {
271                                 if ((msg != "")and(msg2!=""))
272                                         msg = msg ~ ", " ~ msg2;
273                                 else
274                                         msg = msg ~ msg2;
275                                 if ((msg != "")and(msg3!=""))
276                                         msg = msg ~ " and " ~ msg3;
277                                 else
278                                         msg = msg ~ msg3;
279                                 copilot("Instracor:Autopilot disengaged. Careful, check " ~ msg ~ " trim!");
280                         }
281                         me.autothrottle_mode.setValue(0);
282                 }
283                 else
284                         if(me.lateral_mode.getValue() != 2) me.input(0,1);
285         },
286 #################
287         ap_update : func
288         {
289                 var current_alt = getprop("instrumentation/altimeter/indicated-altitude-ft");
290                 var VS = getprop("velocities/vertical-speed-fps");
291                 var TAS = getprop("velocities/uBody-fps");
292                 if(me.step == 0)
293                 { ### glideslope armed ?###
294                         if(me.gs_armed.getValue())
295                         {
296                                 var gsdefl = getprop("instrumentation/nav/gs-needle-deflection");
297                                 var gsrange = getprop("instrumentation/nav/gs-in-range");
298                                 if ((gsdefl< 0.5 and gsdefl>-0.5)and
299                                         gsrange)
300                                 {
301                                         me.vertical_mode.setValue(3);
302                                         me.gs_armed.setValue(0);
303                                 }
304                         }
305                 }
306                 elsif(me.step == 1)
307                 { ### localizer armed ? ###
308                         if(me.loc_armed.getValue())
309                         {
310                                 if (getprop("instrumentation/nav/in-range"))
311                                 {
312
313                                         if(!getprop("instrumentation/nav/nav-loc"))
314                                         {
315                                                 var vheading = getprop("instrumentation/nav/radials/selected-deg");
316                                                 var vvor = getprop("instrumentation/nav/heading-deg");
317                                                 var vdist = getprop("instrumentation/nav/nav-distance");
318                                                 var vorient = getprop("environment/magnetic-variation-deg");
319                                                 var vmag = getprop("orientation/heading-magnetic-deg");
320                                                 var vspeed = getprop("/instrumentation/airspeed-indicator/indicated-mach");
321                                                 var deg_to_rad = math.pi / 180;
322                                                 var vdiff = abs(vheading - vvor + vorient);
323                                                 vdiff = abs(vdist * math.sin(vdiff * deg_to_rad));
324                                                 var vlim = vspeed / 0.3 * 1300 * abs(vheading - vmag) / 45 ;
325                                                 if(vdiff < vlim)
326                                                 {
327                                                         me.lateral_mode.setValue(3);
328                                                         me.loc_armed.setValue(0);
329                                                 }
330                                         }
331                                         else
332                                         {
333                                                 var hddefl = getprop("instrumentation/nav/heading-needle-deflection");
334                                                 if(abs(hddefl) < 9.9)
335                                                 {
336                                                         me.lateral_mode.setValue(3);
337                                                         me.loc_armed.setValue(0);
338                                                         var vradials = getprop("instrumentation/nav[0]/radials/target-radial-deg")
339                                                                 - getprop("environment/magnetic-variation-deg") + 0.5;
340                                                         if(vradials < 0.5) vradials += 360;
341                                                         elsif(vradials >= 360.5) vradials -= 360;
342                                                         me.hdg_setting.setValue(vradials);
343                                                 }
344                                         }
345                                 }
346                         }
347                         elsif(me.lnav_armed.getValue())
348                         {
349                                 if(getprop("position/altitude-agl-ft") > 50)
350                                 {
351                                         me.lnav_armed.setValue(0);              # Clear
352                                         me.lateral_mode.setValue(2);    # LNAV
353                                 }
354                         }
355                 }
356                 elsif(me.step == 2)
357                 { ### check lateral modes  ###
358                         var vheading = getprop("orientation/heading-magnetic-deg");
359                         if(vheading < 0.5)
360                         {
361                                 vheading += 360;
362                         }
363                         me.heading_magnetic.setValue(vheading);
364                         var idx = me.lateral_mode.getValue();
365                         me.AP_roll_mode.setValue(me.roll_list[idx]);
366                         me.AP_roll_engaged.setBoolValue(idx > 0);
367                 }
368                 elsif(me.step == 3)
369                 { ### check vertical modes  ###
370                         var idx = me.vertical_mode.getValue();
371                         var offset = (abs(me.vs_ind.getValue()) / 8);
372                         if(offset < 20)
373                         {
374                                 offset = 20;
375                         }
376                         if((idx==1)or(idx==2))
377                         {
378                                 if (abs(current_alt - me.alt_setting.getValue()) < offset)
379                                 {
380                                         # within MCP altitude: switch to ALT HOLD mode
381                                         idx = 1;        # ALT
382 #                                       if(me.autothrottle_mode.getValue() != 0)
383 #                                       {
384 #                                               me.autothrottle_mode.setValue(5);       # A/T SPD
385 #                                       }
386                                         me.vs_setting.setValue(0);
387                                 }
388                         }
389                         me.vertical_mode.setValue(idx);
390                         me.AP_pitch_mode.setValue(me.pitch_list[idx]);
391                 }
392                 elsif(me.step == 4)                     ### Auto Throttle mode control  ###
393                 {
394                         # Auto throttle arm switch is offed
395                         if(me.at1.getValue() == 0)
396                         {
397                                 me.autothrottle_mode.setValue(0);
398                         }
399                         # Take off mode and above baro 400 ft
400                         elsif(getprop("position/altitude-agl-ft") > 400)
401                         {
402                                 if(me.at1.getValue() == 1)
403                                 {
404                                         me.autothrottle_mode.setValue(1);
405                                 }
406                         }
407                         idx = me.autothrottle_mode.getValue();
408                         me.AP_speed_mode.setValue(me.spd_list[idx]);
409                 }
410                 elsif(me.step == 5)
411                 {
412                         if (getprop("/autopilot/route-manager/active")){
413                                 var max_wpt = getprop("/autopilot/route-manager/route/num");
414                                 var atm_wpt = getprop("/autopilot/route-manager/current-wp");
415                                 var destination_elevation = getprop("/autopilot/route-manager/destination/field-elevation-ft");
416                                 var total_distance = getprop("/autopilot/route-manager/total-distance");
417                                 if(me.lateral_mode.getValue() == 2)             # Current mode is LNAV
418                                 {
419                                         if(atm_wpt < (max_wpt - 1))
420                                         {
421                                                 me.remaining_distance.setValue(getprop("/autopilot/route-manager/wp/remaining-distance-nm")
422                                                         + getprop("autopilot/route-manager/wp/dist"));
423                                                 var next_course = getprop("/autopilot/route-manager/wp[1]/bearing-deg");
424                                         }
425                                         else
426                                         {
427                                                 me.remaining_distance.setValue(getprop("autopilot/route-manager/wp/dist"));
428                                         }
429                                 }
430                                 if(getprop("/autopilot/route-manager/active"))
431                                 {
432                                         var wpt_distance = getprop("autopilot/route-manager/wp/dist");
433                                         var groundspeed = getprop("/velocities/groundspeed-kt");
434                                         if(wpt_distance != nil)
435                                         {
436                                                 var wpt_eta = (wpt_distance / groundspeed * 3600);
437                                                 var gmt = getprop("instrumentation/clock/indicated-sec");
438                                                 if((getprop("gear/gear[1]/wow") == 0) and (getprop("gear/gear[2]/wow") == 0))
439                                                 {
440                                                         gmt += (wpt_eta + 30);
441                                                         var gmt_hour = int(gmt / 3600);
442                                                         if(gmt_hour > 24)
443                                                         {
444                                                                 gmt_hour -= 24;
445                                                                 gmt -= 24 * 3600;
446                                                         }
447                                                         me.estimated_time_arrival.setValue(gmt_hour * 100 + int((gmt - gmt_hour * 3600) / 60));
448                                                         var change_wp = abs(getprop("/autopilot/route-manager/wp[1]/bearing-deg") - me.heading_magnetic.getValue());
449                                                         if(change_wp > 180) change_wp = (360 - change_wp);
450                                                         if(((me.heading_change_rate * change_wp) > wpt_eta)
451                                                                 or (wpt_distance < 0.2)
452                                                                 or ((me.remaining_distance_log_last < wpt_distance) and (change_wp < 80)))
453                                                         {
454                                                                 if(atm_wpt < (max_wpt - 1))
455                                                                 {
456                                                                         atm_wpt += 1;
457                                                                         props.globals.getNode("/autopilot/route-manager/current-wp").setValue(atm_wpt);
458                                                                         me.altitude_restriction = getprop("/autopilot/route-manager/route/wp["~atm_wpt~"]/altitude-ft");
459                                                                 }
460                                                                 me.remaining_distance_log_last = 36000;
461                                                         }
462                                                         else
463                                                         {
464                                                                 me.remaining_distance_log_last = wpt_distance;
465                                                         }
466                                                 }
467                                                 if(getprop("/autopilot/internal/waypoint-bearing-error-deg") != nil)
468                                                 {
469                                                         if(abs(getprop("/position/latitude-deg")) < 80)
470                                                         {
471                                                                 if(abs(getprop("/instrumentation/gps/wp/wp[1]/course-error-nm")) < 2)
472                                                                 {
473                                                                         setprop("/autopilot/internal/course-deviation", getprop("/instrumentation/gps/wp/wp[1]/course-error-nm"))
474                                                                 }
475                                                                 elsif(getprop("/instrumentation/gps/wp/wp[1]/course-deviation-deg") < 2)
476                                                                 {
477                                                                         setprop("/autopilot/internal/course-deviation", getprop("/instrumentation/gps/wp/wp[1]/course-deviation-deg"))
478                                                                 }
479                                                                 else
480                                                                 {
481                                                                         setprop("/autopilot/internal/course-deviation", 0);
482                                                                 }
483                                                         }
484                                                         else
485                                                         {
486                                                                 setprop("/autopilot/internal/course-deviation", 0);
487                                                         }
488                                                 }
489                                         }
490                                 }
491                         }
492                 }
493                 me.step+=1;
494                 if(me.step > 5) me.step = 0;
495         },
496 };
497
498 #####################
499 var click_reset = func(propName) {
500     setprop(propName,0);
501 }
502
503 controls.click = func(button) {
504     if (getprop("sim/freeze/replay-state"))
505         return;
506     var propName="sim/sound/click"~button;
507     setprop(propName,1);
508     settimer(func { click_reset(propName) },0.4);
509 }
510
511 var afds = AFDS.new();
512
513 var afds_init_listener = setlistener("/sim/signals/fdm-initialized", func {
514         removelistener(afds_init_listener);
515         settimer(update_afds,6);
516         print("AFDS System ... check");
517 });
518
519 var update_afds = func {
520         afds.ap_update();
521         settimer(update_afds, 0);
522 }