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();