User:Awexome/PTurn

Procedure Turns - PT

## ##### BEGIN GPL LICENSE BLOCK #####
# This program is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License as
# published by the Free Software Foundation; either version 2 of the
# License, or (at your option) any later version.
#
# And More.
#
# This program is distributed in the hope that it will be useful, but
# WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
# General Public License for more details. <http://www.gnu.org/licenses/>
## ##### BEGIN GPL LICENSE BLOCK #####
#
# Procedure Turns
# Designer/Developer: mijiny <aka awexome[2138]> 201205

# 1 - 45/180 PT; 2 - 90/270
# 2 - 90/270; suitable for situations where there is no navigition to help identify the runway and final approach course.
# Generally, the procedure turn commences at about 10NM from the initial approach fix (IAF), but may extend as farther as 15NM to accommodate performance procedure turns by large and military aircraft, or commence closer to the fix at 5NM where only helicopter and CAT. A are operated.

# 1 - method
# guidanceMode to the initial approach fix (IAF)
# turn onto the outbound course
# fly till 10NM from IAF (5-15NM)
####compute lat,lon of the location using gps trigonometry
####determine heading to the location. GO!

# 2 - method
# head to the PT fix or IAF
# shortly before intercept (5NM), turn onto the outbound course
# at 10NM from the fix, take first turn

## NAVIGATE USING TRACKING MODE (LNAV_TRACK)

LEFT = -1; RIGHT = 1;
pTurnType = {STD:45, NSTD:90};

crsInb = getprop(hpt~"fix-course-deg");
crsOutb = geo.normdeg(crsInb+180);

pTurnDir = RIGHT;
pturnCrsA = geo.normdeg(crsOutb + pTurnType[STD] * (-pTurnDir));
pturnCrsB = geo.normdeg(pturnCrsA + 180 * (pTurnDir));

geoPtA = geo.Coord.new();
geoPtB = geo.Coord.new();

var PTURN = {
init : func(IAF,NAV,) {
  me.testing = 0;
  me.enabled = 0;
  me.running = false;
  me.stage = 0;
  me.loopId = 0;
  me.updateInterval = 1.0; 
},

_loop_ : func {
  me.loopId+=1; # arbitrary counter
  var currStep=getprop("pturn/stage");
  me.running = getprop("pturn/activated");
  
  if(currStep==0) {
    if(distIAF <200) { # 200m
      print("Arrival: IAF.");
      
      # determine geoPtA
      tc = crsOutb;
      d = 10;
      geoIAF = geo.aircraft_position();
      latIAF = geoIAF.latlon()[0]; # geoIAF.lat()
      lonIAF = geoIAF.latlon()[1]; # geoIAF.lon()
      geoPtA = TRJY.calcDistancePoint(tc, d, latIAF, lonIAF);
      
      #turn to the outbound course
      outCourse = geoAC.course_to(geoPtA);    
      # determine wind vector and correct for heading
      goToHdg( geo.normdeg(outCourse + getWCA(outCourse)) );
    
      setprop("pturn/stage",10);
    }
  }
  elsif(currStep==10) {
    # setLocalizer(tgtCourse); # a coord 10NM from IAF
    # props.globals.getNode(hpt~"loc/atrk-mode-on").setBoolValue(1);
    # determine wind vector and correct for heading
    goToHdg( geo.normdeg(outCourse + getWCA(outCourse)) );
    
    geoAC = geo.aircraft_position();
    latAC = geoAC.latlon()[0]; # geoAC.lat()
    lonAC = geoAC.latlon()[1]; # geoAC.lon()
    
    distFromIAF = TRJY.calcGCDistance(latIAF, lonIAF, latAC, lonAC);
  # approx 10NM from IAF (outbound), turn [+/-] 45deg
    if(abs(distFromIAF) => 10) { # 10NM
      if(getprop("acTurning")) setprop("pturn/stage",20);
      setTurn(pturnCrsA, -pTurnDir); # LEFT 45deg
      # goToHdg( geo.normdeg(pturnCrsA + getWCA(pturnCrsA)) );
      # turn onto the outbound course
    }
  }
  elsif(currStep==20) {
    if(!getprop("acTurning")) {
      # start timer
      var flightTime = 0;
      var resetTime = int(getprop("/sim/time/elapsed-sec"));
    
      goToHdg( geo.normdeg(pturnCrsA + getWCA(pturnCrsA)) );
      setprop("pturn/stage",30);
    }
    # use getprop(hpt~"turn/turning")
  }
  elsif(currStep==30){
    goToHdg( geo.normdeg(pturnCrsA + getWCA(pturnCrsA)) );
  # fly the new heading for 1 minute

    flightTime = int(getprop("/sim/time/elapsed-sec")) - resetTime;
    if(flightTime=>60) {
    ## 1 min CAT. A and B
    ## 1.5 min CAT. C, D and E
      if(getprop("acTurning")) setprop("pturn/stage",40);
  # turn 180 degrees !outbound! PT 180-deg turns are headed 'AWAY' from the fix
      setTurn(pturnCrsB, pTurnDir); # turn-flag up!
    }
  }
  elsif(currStep==40) {
    if(!getprop("acTurning")) {
      goToHdg( geo.normdeg(pturnCrsB + getWCA(pturnCrsB)) );
      setprop("pturn/stage",50);
    }
    # use getprop(hpt~"turn/turning")
  }
  elsif(currStep==50){
    hdgError = normDiff(crsInb - getprop("/orientation/heading-deg"));
    if(abs(hdgError)=<45){
# prepare to intercept the inbound course.
      #setLoc(crsInb);
      props.globals.getNode(hpt~"loc/atrk-mode-on").setBoolValue(1); #### use interface objects
      setprop("pturn/stage",60);
    }
  }
  elsif(currStep==60){ # confirm tracking inbound to fix.
    geoAC = geo.aircraft_position();
    latAC = geoAC.latlon()[0]; # geoAC.lat()
    lonAC = geoAC.latlon()[1]; # geoAC.lon()
    distFromIAF = TRJY.calcGCDistance(latIAF, lonIAF, latAC, lonAC);
    
    hdgError = normDiff(getprop(hpt~"fix-course-deg") - getprop("/orientation/heading-deg"));
    if(abs(hdgError)<5 and distFromIAF=<3){
      print("Inbound on IAF. PT completed!");
      setTurn("STOP");
    }
  }
  
  if(me.running.getValue()) settimer(_loop_(), me.updateInterval);
} # update
}; # PTURN


# tracking mode
# track a coord 2 min flight from current position
# dist = Vspd * time
# myPosition = geo.aircraft_position(); # geo.Coord.new();
# compute coord of point at dist at a heading OffHdg from current position
# compute heading to coord
# set course to heading