Aircraft/Citation-Bravo/Nasal/flightdirector.nas

   1 #############################################################################
   2 # Flight Director/Autopilot controller.
   3 # Syd Adams
   4 #
   5 ###  Initialization #######
   6
   7 var Lateral = "autopilot/locks/heading";
   8 var Lateral_arm = "autopilot/locks/heading-arm";
   9 var Vertical = "autopilot/locks/altitude";
  10 var Vertical_arm = "autopilot/locks/altitude-arm";
  11 var AP = "autopilot/locks/AP-status";
  12 var NAVprop="autopilot/settings/nav-source";
  13 var AutoCoord="controls/flight/auto-coordination";
  14 var NAVSRC= getprop(NAVprop);
  15 var count=0;
  16 var Coord = 0;
  17 var minimums=getprop("autopilot/settings/minimums");
  18
  19
  20
  21
  22 #####################################
  23
  24 setlistener("/sim/signals/fdm-initialized", func {
  25     print("Flight Director ...Check");
  26     settimer(update_fd, 30);
  27 });
  28
  29 ### AP /FD BUTTONS ####
  30
  31 var FD_set_mode = func(btn){
  32     var Lmode=getprop(Lateral);
  33     var LAmode=getprop(Lateral_arm);
  34     var Vmode=getprop(Vertical);
  35     var VAmode=getprop(Vertical_arm);
  36
  37
  38     if(btn=="ap"){
  39         Coord = getprop(AutoCoord);
  40         if(getprop(AP)!="AP1"){
  41             setprop(Lateral_arm,"");setprop(Vertical_arm,"");
  42             if(Vmode=="PTCH")set_pitch();
  43             if(Lmode=="ROLL")set_roll();
  44             if(getprop("position/altitude-agl-ft") > minimums) {
  45                 setprop(AP,"AP1");setprop(AutoCoord,0);}
  46         } else kill_AP("");
  47     }elsif(btn=="hdg"){
  48         if(Lmode!="HDG") setprop(Lateral,"HDG") else set_roll();
  49         setprop(Lateral_arm,"");setprop(Vertical_arm,"");
  50     }elsif(btn=="alt"){
  51         setprop(Lateral_arm,"");setprop(Vertical_arm,"");
  52         if(Vmode!="ALT"){
  53             setprop(Vertical,"ALT");setprop("autopilot/settings/asel",(getprop("instrumentation/altimeter/mode-c-alt-ft") * 0.01));
  54         } else set_pitch();
  55     }elsif(btn=="flc"){
  56         var flcmode = "FLC";
  57         var asel = "ASEL";
  58         if(NAVSRC=="FMS"){flcmode="VFLC";asel = "VASEL";}
  59         if(Vmode!=flcmode){
  60             var mc =getprop("instrumentation/airspeed-indicator/indicated-mach");
  61             var kt=int(getprop("instrumentation/airspeed-indicator/indicated-speed-kt"));
  62             if(!getprop("autopilot/settings/changeover")){
  63                 if(kt > 80 and kt <340){
  64                     setprop(Vertical,flcmode);
  65                     setprop(Vertical_arm,asel);
  66                     setprop("autopilot/settings/target-speed-kt",kt);
  67                     setprop("autopilot/settings/target-speed-mach",mc);
  68                 }
  69             }else{
  70                 if(mc > 0.40 and mc <0.85){
  71                     setprop(Vertical,flcmode);
  72                     setprop(Vertical_arm,asel);
  73                     setprop("autopilot/settings/target-speed-kt",kt);
  74                     setprop("autopilot/settings/target-speed-mach",mc);
  75                 }
  76             }
  77         } else set_pitch();
  78     }elsif(btn=="nav"){
  79         set_nav_mode();
  80         setprop("autopilot/settings/low-bank",0);
  81     }elsif(btn=="vnav"){
  82         if(Vmode!="VALT"){
  83             if(NAVSRC=="FMS"){
  84                 setprop(Vertical,"VALT");
  85                 setprop(Lateral,"LNAV");
  86             }
  87             }else set_pitch();
  88     }elsif(btn=="app"){
  89         setprop(Lateral_arm,"");setprop(Vertical_arm,"");
  90         set_apr();
  91         setprop("autopilot/settings/low-bank",0);
  92     }elsif(btn=="vs"){
  93         setprop(Lateral_arm,"");setprop(Vertical_arm,"");
  94          if(Vmode!="VS"){
  95             var tgt_vs = (int(getprop("autopilot/internal/vert-speed-fpm") * 0.01)) * 100;
  96             setprop(Vertical,"VS");setprop("autopilot/settings/vertical-speed-fpm",tgt_vs);
  97         } else set_pitch();
  98     }elsif(btn=="stby"){
  99         setprop(Lateral_arm,"");setprop(Vertical_arm,"");
 100         set_pitch();
 101         set_roll();
 102         setprop("autopilot/settings/low-bank",0);
 103     }elsif(btn=="bank"){
 104         var Bnk="autopilot/settings/low-bank";
 105         if(Lmode=="HDG")setprop(Bnk,1-getprop(Bnk));
 106     }elsif(btn=="co"){
 107         var Co= 1- getprop("autopilot/settings/changeover");
 108          if(Vmode!="FLC") Co=0;
 109          setprop("autopilot/settings/changeover",Co);
 110     }
 111 }
 112
 113 ########  FMS/NAV BUTTONS  ############
 114 var nav_src_set=func(src){
 115     setprop(Lateral_arm,"");setprop(Vertical_arm,"");
 116     set_pitch();set_roll();
 117     if(src=="fms"){
 118         if(getprop("autopilot/route-manager/route/num")>0)setprop(NAVprop,"FMS");
 119     }else{
 120         if (NAVSRC!="NAV1")setprop(NAVprop,"NAV1") else setprop(NAVprop,"NAV2");
 121     }
 122 }
 123
 124 ########### ARM VALID NAV MODE ################
 125
 126 var set_nav_mode=func{
 127     setprop(Lateral_arm,"");setprop(Vertical_arm,"");
 128     if(NAVSRC=="NAV1"){
 129         if(getprop("instrumentation/nav/data-is-valid")){
 130             if(getprop("instrumentation/nav/nav-loc"))setprop(Lateral_arm,"LOC") else setprop(Lateral_arm,"VOR");
 131             setprop(Lateral,"HDG");
 132         }
 133     }elsif(NAVSRC=="NAV2"){
 134         if(getprop("instrumentation/nav[1]/data-is-valid")){
 135             if(getprop("instrumentation/nav[1]/nav-loc"))setprop(Lateral_arm,"LOC") else setprop(Lateral_arm,"VOR");
 136             setprop(Lateral,"HDG");
 137         }
 138     }elsif(NAVSRC=="FMS"){
 139         if(getprop("autopilot/route-manager/active"))setprop(Lateral,"LNAV");
 140     }
 141 }
 142
 143 #######  PITCH WHEEL ACTIONS #############
 144
 145 var pitch_wheel=func(dir){
 146     var Vmode=getprop(Vertical);
 147     var CO = getprop("autopilot/settings/changeover");
 148     var amt=0;
 149     if(Vmode=="VS"){
 150         amt = int(getprop("autopilot/settings/vertical-speed-fpm")) + (dir* 100);
 151         amt = (amt < -8000 ? -8000 : amt > 6000 ? 6000 : amt);
 152         setprop("autopilot/settings/vertical-speed-fpm",amt);
 153     }elsif(Vmode=="FLC" or Vmode=="VFLC"){
 154         if(!CO){
 155             amt=getprop("autopilot/settings/target-speed-kt") + dir;
 156             amt = (amt < 80 ? 80 : amt > 340 ? 340 : amt);
 157             setprop("autopilot/settings/target-speed-kt",amt);
 158         }else{
 159             amt=getprop("autopilot/settings/target-speed-mach") + (dir*0.01);
 160             amt = (amt < 0.40 ? 0.40 : amt > 0.85 ? 0.85 : amt);
 161             setprop("autopilot/settings/target-speed-mach",amt);
 162         }
 163     }elsif(Vmode=="PTCH"){
 164         amt=getprop("autopilot/settings/target-pitch-deg") + (dir*0.1);
 165         amt = (amt < -20 ? -20 : amt > 20 ? 20 : amt);
 166         setprop("autopilot/settings/target-pitch-deg",amt)
 167     }
 168 }
 169
 170 ########    FD INTERNAL ACTIONS  #############
 171
 172 var set_pitch=func{
 173     setprop(Vertical,"PTCH");setprop("autopilot/settings/target-pitch-deg",getprop("orientation/pitch-deg"));
 174 }
 175
 176 var set_roll=func{
 177     setprop(Lateral,"ROLL");setprop("autopilot/settings/target-roll-deg",0.0);
 178 }
 179
 180 var set_apr=func{
 181     if(NAVSRC == "NAV1"){
 182         if(getprop("instrumentation/nav/nav-loc") and getprop("instrumentation/nav/has-gs")){
 183             setprop(Lateral_arm,"LOC");
 184             setprop(Vertical_arm,"GS");
 185             setprop(Lateral,"HDG");
 186         }
 187     }elsif(NAVSRC == "NAV2"){
 188     if(getprop("instrumentation/nav[1]/nav-loc") and getprop("instrumentation/nav[1]/has-gs")){
 189             setprop(Lateral_arm,"LOC");
 190             setprop(Vertical_arm,"GS");
 191             setprop(Lateral,"HDG");
 192         }
 193     }
 194 }
 195
 196 setlistener("autopilot/settings/minimums", func(mn) {
 197     minimums=mn.getValue();
 198 },1,0);
 199
 200 setlistener("/autopilot/settings/low-bank", func(Bk) {
 201     var lmt = [27,17];
 202     setprop("autopilot/settings/bank-limit",lmt[Bk.getValue()]);
 203 },1,0);
 204
 205 setlistener(NAVprop, func(Nv) {
 206     NAVSRC=Nv.getValue();
 207 },1,0);
 208
 209 var update_nav=func{
 210     var sgnl = "- - -";
 211     var gs =0;
 212     if(NAVSRC == "NAV1"){
 213         if(getprop("instrumentation/nav/data-is-valid"))sgnl="VOR1";
 214         setprop("autopilot/internal/in-range",getprop("instrumentation/nav/in-range"));
 215         setprop("autopilot/internal/gs-in-range",getprop("instrumentation/nav/gs-in-range"));
 216         var dst=getprop("instrumentation/nav/nav-distance") or 0;
 217         dst*=0.000539;
 218         setprop("autopilot/internal/nav-distance",dst);
 219         setprop("autopilot/internal/nav-id",getprop("instrumentation/nav/nav-id"));
 220         if(getprop("instrumentation/nav/nav-loc"))sgnl="LOC1";
 221         if(getprop("instrumentation/nav/has-gs"))sgnl="ILS1";
 222         if(sgnl=="ILS1")gs = 1;
 223         setprop("autopilot/internal/gs-valid",gs);
 224         setprop("autopilot/internal/nav-type",sgnl);
 225         course_offset("instrumentation/nav[0]/radials/selected-deg");
 226         setprop("autopilot/internal/to-flag",getprop("instrumentation/nav/to-flag"));
 227         setprop("autopilot/internal/from-flag",getprop("instrumentation/nav/from-flag"));
 228     }elsif(NAVSRC == "NAV2"){
 229         if(getprop("instrumentation/nav[1]/data-is-valid"))sgnl="VOR2";
 230         setprop("autopilot/internal/in-range",getprop("instrumentation/nav[1]/in-range"));
 231         setprop("autopilot/internal/gs-in-range",getprop("instrumentation/nav[1]/gs-in-range"));
 232         var dst=getprop("instrumentation/nav[1]/nav-distance") or 0;
 233         dst*=0.000539;
 234         setprop("autopilot/internal/nav-distance",dst);
 235         setprop("autopilot/internal/nav-id",getprop("instrumentation/nav[1]/nav-id"));
 236         if(getprop("instrumentation/nav[1]/nav-loc"))sgnl="LOC2";
 237         if(getprop("instrumentation/nav[1]/has-gs"))sgnl="ILS2";
 238         if(sgnl=="ILS2")gs = 1;
 239         setprop("autopilot/internal/gs-valid",gs);
 240         setprop("autopilot/internal/nav-type",sgnl);
 241         course_offset("instrumentation/nav[1]/radials/selected-deg");
 242         setprop("autopilot/internal/to-flag",getprop("instrumentation/nav[1]/to-flag"));
 243         setprop("autopilot/internal/from-flag",getprop("instrumentation/nav[1]/from-flag"));
 244     }elsif(NAVSRC == "FMS"){
 245         setprop("autopilot/internal/nav-type","FMS1");
 246         setprop("autopilot/internal/in-range",1);
 247         setprop("autopilot/internal/gs-in-range",0);
 248         setprop("autopilot/internal/nav-distance",getprop("instrumentation/gps/wp/wp[1]/distance-nm"));
 249         setprop("autopilot/internal/nav-id",getprop("instrumentation/gps/wp/wp[1]/ID"));
 250         course_offset("instrumentation/gps/wp/wp[1]/bearing-mag-deg");
 251         setprop("autopilot/internal/to-flag",getprop("instrumentation/gps/wp/wp[1]/to-flag"));
 252         setprop("autopilot/internal/from-flag",getprop("instrumentation/gps/wp/wp[1]/from-flag"));
 253     }
 254 }
 255
 256 var course_offset = func(src){
 257     var crs_set=getprop(src);
 258     var crs_offset= crs_set - getprop("orientation/heading-magnetic-deg");
 259     if(crs_offset>180)crs_offset-=360;
 260     if(crs_offset<-180)crs_offset+=360;
 261     setprop("autopilot/internal/course-offset",crs_offset);
 262     crs_offset+=getprop("autopilot/internal/cdi");
 263     if(crs_offset>180)crs_offset-=360;
 264     if(crs_offset<-180)crs_offset+=360;
 265     setprop("autopilot/internal/ap_crs",crs_offset);
 266     setprop("autopilot/internal/selected-crs",crs_set);
 267 }
 268
 269 var monitor_L_armed = func{
 270     if(getprop(Lateral_arm)!=""){
 271         if(getprop("autopilot/internal/in-range")){
 272             var cdi=getprop("autopilot/internal/cdi");
 273             if(cdi < 40 and cdi > -40){
 274                 setprop(Lateral,getprop(Lateral_arm));
 275                 setprop(Lateral_arm,"");
 276             }
 277         }
 278     }
 279 }
 280
 281 var monitor_V_armed = func{
 282     var Varm=getprop(Vertical_arm);
 283     var myalt=getprop("instrumentation/altimeter/indicated-altitude-ft");
 284     var asel=getprop("autopilot/settings/asel") * 100;
 285     var alterr=myalt-asel;
 286     if(Varm=="ASEL"){
 287         if(alterr >-250 and alterr <250){
 288             setprop(Vertical,"ALT");
 289             setprop(Vertical_arm,"");
 290         }
 291     }elsif(Varm=="VASEL"){
 292         if(alterr >-250 and alterr <250){
 293             setprop(Vertical,"VALT");
 294             setprop("instrumentation/gps/wp/wp[1]/altitude-ft",asel);
 295             setprop(Vertical_arm,"");
 296         }
 297     }elsif(Varm=="GS"){
 298         if(getprop(Lateral)=="LOC"){
 299             if(getprop("autopilot/internal/gs-in-range")){
 300                 var gs_err=getprop("autopilot/internal/gs-deflection");
 301                 var gs_dst=getprop("autopilot/internal/nav-distance");
 302                 if(gs_dst <= 7.0){
 303                     if(gs_err >-0.25 and gs_err < 0.25){
 304                         setprop(Vertical,"GS");
 305                         setprop(Vertical_arm,"");
 306                     }
 307                 }
 308             }
 309         }
 310     }
 311 }
 312
 313 var monitor_AP_errors= func{
 314     var ralt=getprop("position/altitude-agl-ft");
 315     if(ralt<minimums)kill_Ap("");
 316     var rlimit=getprop("orientation/roll-deg");
 317     if(rlimit > 40 or rlimit< -40)kill_Ap("AP-FAIL");
 318     var plimit=getprop("orientation/pitch-deg");
 319     if(plimit > 25 or plimit< -25)kill_Ap("AP-FAIL");
 320 }
 321
 322 var kill_Ap = func(msg){
 323     setprop(AP,msg);
 324     setprop(AutoCoord,Coord);
 325 }
 326
 327 var get_ETE= func{
 328     var ttw = "--:--";
 329     var min =0;
 330     var hr=0;
 331     if(NAVSRC == "NAV1"){
 332         setprop("instrumentation/dme/frequencies/source","instrumentation/nav/frequencies/selected-mhz");
 333         min = int(getprop("instrumentation/dme/indicated-time-min"));
 334         if(min>60){
 335             var tmphr=(min*0.016666);
 336             hr=int(tmphr);
 337             var tmpmin=(tmphr-hr)*100;
 338             min=int(tmpmin);
 339         }
 340         ttw=sprintf("ETE %i:%02i",hr,min);
 341     }elsif(NAVSRC == "NAV2"){
 342         setprop("instrumentation/dme/frequencies/source","instrumentation/nav[1]/frequencies/selected-mhz");
 343         min = int(getprop("instrumentation/dme/indicated-time-min"));
 344         if(min>60){
 345             var tmphr=(min*0.016666);
 346             hr=int(tmphr);
 347             var tmpmin=(tmphr-hr)*100;
 348             min=int(tmpmin);
 349         }
 350         ttw=sprintf("ETE %s:%02i",hr,min);
 351     }elsif(NAVSRC == "FMS"){
 352         min = getprop("autopilot/route-manager/ete");
 353         min=int(min * 0.016666);
 354         if(min>60){
 355             var tmphr=(min*0.016666);
 356             hr=int(tmphr);
 357             var tmpmin=(tmphr-hr)*100;
 358             min=int(tmpmin);
 359         }
 360          ttw=sprintf("ETE %s:%02i",hr,min);
 361     }
 362     setprop("autopilot/internal/nav-ttw",ttw);
 363 }
 364
 365
 366 ###  Main loop ###
 367
 368 var update_fd = func {
 369     update_nav();
 370     if(count==0)monitor_AP_errors();
 371     if(count==1)monitor_L_armed();
 372     if(count==2)monitor_V_armed();
 373      if(count==3)get_ETE();
 374     count+=1;
 375     if(count>3)count=0;
 376     settimer(update_fd, 0);
 377 }