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