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