Syd Adams:
[fg:toms-fgdata.git] / Aircraft / Citation-Bravo / Nasal / flightdirector.nas
1 #############################################################################
2 #
3 # Generic Flight Director.
4 #
5 # Written by Syd Adams
6 #Modification of Curtis Olson's flightdirector flight director.
7 # Started 30 Jan 2006.
8 #
9 #############################################################################
10
11 #############################################################################
12 # Global shared variables
13 #############################################################################
14
15 # off - Off: v-bars hidden
16 # fd - power  on , no selected modes ...AP defaults to winglevel + pitch at the
17 #      time mode was activated.
18 # hdg - Headingbug mode
19 # appr - Approach: bank and pitch commands capture and track LOC and GS
20 #        (or just bank if VOR/RNAV)
21 # bc - Reverse Localizer: bank command to caputre and track a reverse LOC
22 #      course.  GS is locked out.
23 # arm - Standby mode to compute capture point for nav, appr, or bc.
24 # cpld - Coupled: Active mode for nav, appr, or bc.
25 # ga - Go Around: commands wings level and missed approach attitude.
26 # alt - Altitude hold: commands pitch to hold altitude
27 # vertical trim - pitch command to adjust altitude at 500 fpm while in alt hold
28 #                 or pitch attitude at rate of 1 deg/sec when not in alt hold
29 # ap - Autopilot Engage: AP will attempt to track FD.
30 # pitch synchronization: manual flight maneuvering without the need to 
31 #                        disengage/engage the AP/FD, AP maintains pitch axis.
32 # yd - Yaw Damper: system senses motion around ayw axis and moves rudder to
33 #                  oppose yaw.
34
35 fdmode = "off";
36 setprop("/instrumentation/flightdirector/fdmode", fdmode);
37 fdmode_last = "off";
38 vbar_roll = 0.0;
39 vbar_pitch = 0.0;
40 vbar_rol_propl = 0.0;
41 vbar_pitch_prop = 0.0;
42 nav_dist = 0.0;
43 last_nav_dist = 0.0;
44 last_nav_time = 0.0;
45 tth_filter = 0.0;
46 alt_select = 0.0;
47 current_alt=0.0;
48 alt_offset = 0.0;
49 kfcmode="";
50 ap_on = 0.0;
51 alt_alert = 0.0;
52
53
54 #############################################################################
55 # Use tha nasal timer to call the initialization function once the sim is
56 # up and running
57 #############################################################################
58
59 INIT = func {
60     # default values
61     print("Initializing Flight Director");
62     setprop("/instrumentation/flightdirector/fdmode", "off");
63     setprop("/instrumentation/flightdirector/vbar-pitch", 0.0);
64     setprop("/instrumentation/flightdirector/vbar-roll", 0.0);
65     setprop("/instrumentation/flightdirector/alt-offset", 0.0);
66     setprop("/instrumentation/flightdirector/autopilot-on",0.0);
67     setprop("/instrumentation/flightdirector/alt-alert", alt_alert);
68     setprop("/autopilot/settings/heading-bug-deg",0);
69     setprop("/autopilot/settings/target-altitude-ft",0);
70 current_alt = getprop("/instrumentation/altimer/indicated-altitude-ft");
71 alt_select = getprop("/autopilot/settings/target-altitude-ft");
72
73 }
74 settimer(INIT, 0);
75
76
77 #############################################################################
78 # handle KC 290 Mode Controller inputs, and compute correct mode/settings
79 #############################################################################
80
81 handle_inputs = func {
82 # Autopilot  activate
83     fdmode = getprop("/instrumentation/flightdirector/fdmode");
84    ap_on = getprop("/instrumentation/flightdirector/autopilot-on");
85
86 if(ap_on==1){
87 if(fdmode == "fd" or fdmode == "off"){setprop("autopilot/locks/heading","wing-leveler");}
88 if(fdmode == "hdg"){setprop("autopilot/locks/heading","dg-heading-hold");}
89 if(fdmode == "alt"){setprop("autopilot/locks/altitude","altitude-hold");}
90 }
91 if(ap_on == 0){
92 setprop("autopilot/locks/heading","");
93 setprop("autopilot/locks/altitude","");
94 setprop("autopilot/locks/speed","");
95   }
96 }
97
98
99 #############################################################################
100 # track and update mode
101 #############################################################################
102
103 update_mode = func {
104     fdmode = getprop("/instrumentation/flightdirector/fdmode");
105
106     # compute elapsed time since last iteration
107     nav_time = getprop("/sim/time/elapsed-sec");
108     nav_dt = nav_time - last_nav_time;
109     last_nav_time = nav_time;
110
111     inrange = getprop("/instrumentation/nav/in-range");
112     if ( inrange ) {
113         # compute distance to nav heading intercept
114         nav_dist = getprop("/instrumentation/nav/crosstrack-error-m");
115
116         # compute time to heading (tth)
117         nav_rate = (last_nav_dist - nav_dist) / nav_dt;
118         if ( abs(nav_rate) > 0.00001 ) {
119             tth = nav_dist / nav_rate;
120         } else {
121             tth = 9999.9;
122         }
123       #  print("nav-dist = ", nav_dist, " tth = ", tth);
124
125         tth_filter = 0.9 * tth_filter + 0.1 * tth;
126         last_nav_dist = nav_dist;
127     }        
128
129     if ( fdmode == "nav-arm" ) {
130         curhdg = getprop("/orientation/heading-magnetic-deg");
131         tgtrad = getprop("/instrumentation/nav/radials/selected-deg");
132         if ( tgtrad == nil or tgtrad == "" ) {
133             tgtrad = 0.0;
134         }
135         diff = tgtrad - curhdg;
136         if ( diff < -180.0 ) {
137             diff += 360.0;
138         } elsif ( diff > 180.0 ) {
139             diff -= 180.0;
140         }
141
142         # standard rate turn is 3 dec/sec
143         roll_out_time_sec = abs(diff) / 3.0;
144
145       #  print("tth = ", tth_filter, " hdgdiff = ", diff, " rollout = ", roll_out_time_sec );
146         if ( roll_out_time_sec >= abs(tth_filter) ) {
147             # switch from arm to cpld
148             fdmode = "nav-cpld";
149         }
150
151     }
152
153     setprop("/instrumentation/flightdirector/fdmode", fdmode);
154 }
155
156 #############################################################################
157 #get pitch from autopilot altitude setting
158 #############################################################################
159
160 get_altpitch = func(){
161 alt_offset = 0.0;
162 alt_select = getprop("/autopilot/settings/target-altitude-ft");
163 if ( alt_select == nil or alt_select == "" ){ alt_select = 0.0;return (alt_select);}
164 current_alt = getprop("/instrumentation/altimeter/indicated-altitude-ft");
165 if(current_alt == nil){current_alt = 0.0;}
166 alt_offset = (alt_select-current_alt);
167 setprop("/instrumentation/flightdirector/alt-alert",alt_offset);
168 if(alt_offset > 500.0){alt_offset = 500.0;}
169 if(alt_offset < -500.0){alt_offset = -500.0;}
170 vbar_pitch = alt_offset * 0.01;
171 }
172
173
174 #############################################################################
175 # update the FD vbar position for the various modes
176 #############################################################################
177
178 update_vbar = func {
179     if ( fdmode == "fd" ) {
180         # wings level maintain pitch at time of mode activation
181         if ( fdmode_last != "fd" ) {
182             vbar_roll = 0.0;
183            # vbar_pitch = getprop("/orientation/pitch-deg");
184 }
185     } elsif ( fdmode == "hdg" or fdmode == "nav-arm" ) {
186         # FIXME: at what angle off of the hdg bug do we start the rollout?
187         # bank to track heading bug
188 vbar_pitch =  get_altpitch();
189
190         if ( fdmode_last != "hdg" ) {
191
192         }
193         tgtrad = getprop("/autopilot/settings/heading-bug-deg");
194         if ( tgtrad == nil or tgtrad == "" ) {
195             tgtrad = 0.0;
196         }
197         curhdg = getprop("/orientation/heading-magnetic-deg");
198         diff = tgtrad - curhdg;
199         if ( diff < -180.0 ) {
200             diff += 360.0;
201         } elsif ( diff > 180.0 ) {
202             diff -= 180.0;
203         }
204         # max bank = 30, so this means roll out begins at 15 dgs off target hdg
205         bank = 2 * diff;
206         if ( bank < -30.0 ) {
207             bank = -30.0;
208         }
209         if ( bank > 30.0 ) {
210             bank = 30.0;
211         }
212         vbar_roll = bank;
213 #        print("diff = ", diff);
214     } elsif ( fdmode == "nav-cpld" ) {
215         curhdg = getprop("/orientation/heading-magnetic-deg");
216         tgtrad = getprop("/instrumentation/nav/radials/selected-deg");
217         toflag = getprop("/instrumentation/nav/to-flag");
218         actrad = 0.0;
219         offset = 0.0;
220         if ( toflag ) {
221             actrad = getprop("/instrumentation/nav/radials/reciprocal-radial-deg");
222             offset = 3 * (actrad - tgtrad);
223         } else {
224             actrad = getprop("/instrumentation/nav/radials/actual-deg");
225             offset = 3 * (tgtrad - actrad);
226         }
227
228         if ( offset < -90.0 ) { offset = -90.0; }
229         if ( offset > 90.0 ) { offset = 90.0; }
230         tgthdg = tgtrad + offset;
231
232         diff = tgthdg - curhdg;
233         if ( diff < -180.0 ) {
234             diff += 360.0;
235         } elsif ( diff > 180.0 ) {
236             diff -= 180.0;
237         }
238        # print("* offset = ", offset, " tgthdg = ", tgthdg, " diff = ", diff);
239
240         # max bank = 30, so this means roll out begins at 15 dgs off target hdg
241         bank = 2 * diff;
242         if ( bank < -30.0 ) {
243             bank = -30.0;
244         }
245         if ( bank > 30.0 ) {
246             bank = 30.0;
247         }
248         vbar_roll = bank;
249
250     } else {
251         # assume off if nothing else specified, and hide vbars
252         vbar_roll = 0.0;
253 get_altpitch();
254     }
255
256
257     fdmode_last = fdmode;
258 vbar_pitch_prop = (vbar_pitch-getprop("/orientation/pitch-deg"));
259 vbar_roll_prop = (getprop("/orientation/roll-deg") - vbar_roll);
260 if(vbar_roll_prop > 30.0){vbar_roll_prop = 30.0;}
261 if(vbar_roll_prop < -30.0){vbar_roll_prop = -30.0;}
262 if(vbar_pitch_prop > 5.0){vbar_pitch_prop = 5.0;}
263 if(vbar_pitch_prop < -5.0){vbar_pitch_prop = -5.0;}
264
265
266 setprop("/instrumentation/flightdirector/vbar-pitch",vbar_pitch_prop);
267 setprop("/instrumentation/flightdirector/vbar-roll", vbar_roll_prop);
268 setprop("/instrumentation/flightdirector/alt-offset", alt_offset);
269 setprop("/instrumentation/flightdirector/current-alt", current_alt);
270 }
271
272
273 #############################################################################
274 # main update function to be called each frame
275 #############################################################################
276
277 update = func {
278 #    print("kfc-200 update");
279
280     handle_inputs();
281     update_mode();
282     update_vbar();
283
284 #    print( "vbar roll = ", vbar_roll, "(", getprop("/orientation/roll-deg"),
285  #          ") pitch = ", vbar_pitch, "(", getprop("/orientation/pitch-deg"),
286  #         ")" );
287
288     registerTimer();
289 }
290
291
292 #############################################################################
293 # Use tha nasal timer to call ourselves every frame
294 #############################################################################
295
296 registerTimer = func {
297     settimer(update, 0);
298 }
299 registerTimer();