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