Fix radio stby freq can not be set
[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                                                         # set target to current magnetic heading
229                                                         var tgtHdg = int(me.heading_magnetic.getValue() + 0.50);
230                                                         me.hdg_setting.setValue(tgtHdg);
231                                                         me.lateral_mode.setValue(1);            # HDG HOLD
232                                                 }
233                                                 if(me.vertical_mode.getValue() == 0)    # Not set
234                                                 {
235                                                         var alt = int((current_alt+50)/100)*100;
236                                                         me.target_alt.setValue(alt);
237                                                         me.vertical_mode.setValue(1);           # ALT
238                                                 }
239                                                 me.FD.setValue(1)
240                                         }
241                                         else
242                                         {
243                                                 if(!me.AP.getValue())
244                                                 {
245                                                         me.lateral_mode.setValue(0);            # Clear
246                                                         me.vertical_mode.setValue(0);           # Clear
247                                                 }
248                                                 me.FD.setValue(0)
249                                         }
250                                 }
251                         }
252 #               }
253         },
254 ###################
255         setAP : func{
256                 var output = 1-me.AP.getValue();
257                 var disabled = me.AP_disengaged.getValue();
258                 if((output==0)and(getprop("position/altitude-agl-ft")<200))
259                 {
260                         disabled = 1;
261                         copilot("Instractor:Autopilot won't engage below 200ft.");
262                 }
263                 if((disabled)and(output==0)){output = 1;me.AP.setValue(0);}
264                 if (output==1)
265                 {
266                         var msg="";
267                         var msg2="";
268                         var msg3="";
269                         if (abs(getprop("controls/flight/rudder-trim"))   > 0.04) msg  = "rudder";
270                         if (abs(getprop("controls/flight/elevator-trim")) > 0.04) msg2 = "pitch";
271                         if (abs(getprop("controls/flight/aileron-trim"))  > 0.04) msg3 = "aileron";
272                         if (msg ~ msg2 ~ msg3 != "")
273                         {
274                                 if ((msg != "")and(msg2!=""))
275                                         msg = msg ~ ", " ~ msg2;
276                                 else
277                                         msg = msg ~ msg2;
278                                 if ((msg != "")and(msg3!=""))
279                                         msg = msg ~ " and " ~ msg3;
280                                 else
281                                         msg = msg ~ msg3;
282                                 copilot("Instracor:Autopilot disengaged. Careful, check " ~ msg ~ " trim!");
283                         }
284                         me.loc_armed.setValue(0);                       # Disarm
285                         me.gs_armed.setValue(0);                        # Disarm
286                         if(!me.FD.getValue())
287                         {
288                                 me.lateral_mode.setValue(0);            # NO MODE
289                                 me.vertical_mode.setValue(0);           # NO MODE
290                         }
291                         else
292                         {
293                                 me.lateral_mode.setValue(1);            # HDG HOLD
294                                 me.vertical_mode.setValue(1);           # ALT
295                         }
296                         me.autothrottle_mode.setValue(0);
297                 }
298                 else
299                 {
300                         if(me.lateral_mode.getValue() == 0)             # Not set
301                         {
302                                 me.input(0,1);
303                         }
304                         if(me.vertical_mode.getValue() == 0)    # Not set
305                         {
306                                 me.input(1,1);
307                         }
308                 }
309         },
310 #################
311         ap_update : func
312         {
313                 var current_alt = getprop("instrumentation/altimeter/indicated-altitude-ft");
314                 var VS = getprop("velocities/vertical-speed-fps");
315                 var TAS = getprop("velocities/uBody-fps");
316                 if(me.step == 0)
317                 { ### glideslope armed ?###
318                         if(me.gs_armed.getValue())
319                         {
320                                 var gsdefl = getprop("instrumentation/nav/gs-needle-deflection");
321                                 var gsrange = getprop("instrumentation/nav/gs-in-range");
322                                 if ((gsdefl< 0.5 and gsdefl>-0.5)and
323                                         gsrange)
324                                 {
325                                         me.vertical_mode.setValue(3);
326                                         me.gs_armed.setValue(0);
327                                 }
328                         }
329                 }
330                 elsif(me.step == 1)
331                 { ### localizer armed ? ###
332                         if(me.loc_armed.getValue())
333                         {
334                                 if (getprop("instrumentation/nav/in-range"))
335                                 {
336
337                                         if(!getprop("instrumentation/nav/nav-loc"))
338                                         {
339                                                 var vheading = getprop("instrumentation/nav/radials/selected-deg");
340                                                 var vvor = getprop("instrumentation/nav/heading-deg");
341                                                 var vdist = getprop("instrumentation/nav/nav-distance");
342                                                 var vorient = getprop("environment/magnetic-variation-deg");
343                                                 var vmag = getprop("orientation/heading-magnetic-deg");
344                                                 var vspeed = getprop("/instrumentation/airspeed-indicator/indicated-mach");
345                                                 var deg_to_rad = math.pi / 180;
346                                                 var vdiff = abs(vheading - vvor + vorient);
347                                                 vdiff = abs(vdist * math.sin(vdiff * deg_to_rad));
348                                                 var vlim = vspeed / 0.3 * 1300 * abs(vheading - vmag) / 45 ;
349                                                 if(vdiff < vlim)
350                                                 {
351                                                         me.lateral_mode.setValue(3);
352                                                         me.loc_armed.setValue(0);
353                                                 }
354                                         }
355                                         else
356                                         {
357                                                 var hddefl = getprop("instrumentation/nav/heading-needle-deflection");
358                                                 if(abs(hddefl) < 9.9)
359                                                 {
360                                                         me.lateral_mode.setValue(3);
361                                                         me.loc_armed.setValue(0);
362                                                         var vradials = getprop("instrumentation/nav[0]/radials/target-radial-deg")
363                                                                 - getprop("environment/magnetic-variation-deg") + 0.5;
364                                                         if(vradials < 0.5) vradials += 360;
365                                                         elsif(vradials >= 360.5) vradials -= 360;
366                                                         me.hdg_setting.setValue(vradials);
367                                                 }
368                                         }
369                                 }
370                         }
371                         elsif(me.lnav_armed.getValue())
372                         {
373                                 if(getprop("position/altitude-agl-ft") > 50)
374                                 {
375                                         me.lnav_armed.setValue(0);              # Clear
376                                         me.lateral_mode.setValue(2);    # LNAV
377                                 }
378                         }
379                 }
380                 elsif(me.step == 2)
381                 { ### check lateral modes  ###
382                         var vheading = getprop("orientation/heading-magnetic-deg");
383                         if(vheading < 0.5)
384                         {
385                                 vheading += 360;
386                         }
387                         me.heading_magnetic.setValue(vheading);
388                         var idx = me.lateral_mode.getValue();
389                         me.AP_roll_mode.setValue(me.roll_list[idx]);
390                         me.AP_roll_engaged.setBoolValue(idx > 0);
391                 }
392                 elsif(me.step == 3)
393                 { ### check vertical modes  ###
394                         var idx = me.vertical_mode.getValue();
395                         var offset = (abs(me.vs_ind.getValue()) / 8);
396                         if(offset < 20)
397                         {
398                                 offset = 20;
399                         }
400                         if((idx==1)or(idx==2))
401                         {
402                                 if (abs(current_alt - me.alt_setting.getValue()) < offset)
403                                 {
404                                         # within MCP altitude: switch to ALT HOLD mode
405                                         idx = 1;        # ALT
406 #                                       if(me.autothrottle_mode.getValue() != 0)
407 #                                       {
408 #                                               me.autothrottle_mode.setValue(5);       # A/T SPD
409 #                                       }
410                                         me.vs_setting.setValue(0);
411                                 }
412                         }
413                         me.vertical_mode.setValue(idx);
414                         me.AP_pitch_mode.setValue(me.pitch_list[idx]);
415                 }
416                 elsif(me.step == 4)                     ### Auto Throttle mode control  ###
417                 {
418                         # Auto throttle arm switch is offed
419                         if(me.at1.getValue() == 0)
420                         {
421                                 me.autothrottle_mode.setValue(0);
422                         }
423                         # Take off mode and above baro 400 ft
424                         elsif(getprop("position/altitude-agl-ft") > 400)
425                         {
426                                 if(me.at1.getValue() == 1)
427                                 {
428                                         me.autothrottle_mode.setValue(1);
429                                 }
430                         }
431                         idx = me.autothrottle_mode.getValue();
432                         me.AP_speed_mode.setValue(me.spd_list[idx]);
433                 }
434                 elsif(me.step == 5)
435                 {
436                         if (getprop("/autopilot/route-manager/active")){
437                                 var max_wpt = getprop("/autopilot/route-manager/route/num");
438                                 var atm_wpt = getprop("/autopilot/route-manager/current-wp");
439                                 var destination_elevation = getprop("/autopilot/route-manager/destination/field-elevation-ft");
440                                 var total_distance = getprop("/autopilot/route-manager/total-distance");
441                                 if(me.lateral_mode.getValue() == 2)             # Current mode is LNAV
442                                 {
443                                         if(atm_wpt < (max_wpt - 1))
444                                         {
445                                                 me.remaining_distance.setValue(getprop("/autopilot/route-manager/wp/remaining-distance-nm")
446                                                         + getprop("autopilot/route-manager/wp/dist"));
447                                                 var next_course = getprop("/autopilot/route-manager/wp[1]/bearing-deg");
448                                         }
449                                         else
450                                         {
451                                                 me.remaining_distance.setValue(getprop("autopilot/route-manager/wp/dist"));
452                                         }
453                                 }
454                                 if(getprop("/autopilot/route-manager/active"))
455                                 {
456                                         var wpt_distance = getprop("autopilot/route-manager/wp/dist");
457                                         var groundspeed = getprop("/velocities/groundspeed-kt");
458                                         if(wpt_distance != nil)
459                                         {
460                                                 var wpt_eta = (wpt_distance / groundspeed * 3600);
461                                                 var gmt = getprop("instrumentation/clock/indicated-sec");
462                                                 if((getprop("gear/gear[1]/wow") == 0) and (getprop("gear/gear[2]/wow") == 0))
463                                                 {
464                                                         gmt += (wpt_eta + 30);
465                                                         var gmt_hour = int(gmt / 3600);
466                                                         if(gmt_hour > 24)
467                                                         {
468                                                                 gmt_hour -= 24;
469                                                                 gmt -= 24 * 3600;
470                                                         }
471                                                         me.estimated_time_arrival.setValue(gmt_hour * 100 + int((gmt - gmt_hour * 3600) / 60));
472                                                         var change_wp = abs(getprop("/autopilot/route-manager/wp[1]/bearing-deg") - me.heading_magnetic.getValue());
473                                                         if(change_wp > 180) change_wp = (360 - change_wp);
474                                                         if(((me.heading_change_rate * change_wp) > wpt_eta)
475                                                                 or (wpt_distance < 0.2)
476                                                                 or ((me.remaining_distance_log_last < wpt_distance) and (change_wp < 80)))
477                                                         {
478                                                                 if(atm_wpt < (max_wpt - 1))
479                                                                 {
480                                                                         atm_wpt += 1;
481                                                                         props.globals.getNode("/autopilot/route-manager/current-wp").setValue(atm_wpt);
482                                                                         me.altitude_restriction = getprop("/autopilot/route-manager/route/wp["~atm_wpt~"]/altitude-ft");
483                                                                 }
484                                                                 me.remaining_distance_log_last = 36000;
485                                                         }
486                                                         else
487                                                         {
488                                                                 me.remaining_distance_log_last = wpt_distance;
489                                                         }
490                                                 }
491                                                 if(getprop("/autopilot/internal/waypoint-bearing-error-deg") != nil)
492                                                 {
493                                                         if(abs(getprop("/position/latitude-deg")) < 80)
494                                                         {
495                                                                 if(abs(getprop("/instrumentation/gps/wp/wp[1]/course-error-nm")) < 2)
496                                                                 {
497                                                                         setprop("/autopilot/internal/course-deviation", getprop("/instrumentation/gps/wp/wp[1]/course-error-nm"))
498                                                                 }
499                                                                 elsif(getprop("/instrumentation/gps/wp/wp[1]/course-deviation-deg") < 2)
500                                                                 {
501                                                                         setprop("/autopilot/internal/course-deviation", getprop("/instrumentation/gps/wp/wp[1]/course-deviation-deg"))
502                                                                 }
503                                                                 else
504                                                                 {
505                                                                         setprop("/autopilot/internal/course-deviation", 0);
506                                                                 }
507                                                         }
508                                                         else
509                                                         {
510                                                                 setprop("/autopilot/internal/course-deviation", 0);
511                                                         }
512                                                 }
513                                         }
514                                 }
515                         }
516                 }
517                 me.step+=1;
518                 if(me.step > 5) me.step = 0;
519         },
520 };
521
522 #####################
523 var click_reset = func(propName) {
524     setprop(propName,0);
525 }
526
527 controls.click = func(button) {
528     if (getprop("sim/freeze/replay-state"))
529         return;
530     var propName="sim/sound/click"~button;
531     setprop(propName,1);
532     settimer(func { click_reset(propName) },0.4);
533 }
534
535 var afds = AFDS.new();
536
537 var afds_init_listener = setlistener("/sim/signals/fdm-initialized", func {
538         removelistener(afds_init_listener);
539         settimer(update_afds,6);
540         print("AFDS System ... check");
541 });
542
543 var update_afds = func {
544         afds.ap_update();
545         settimer(update_afds, 0);
546 }