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