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