/** * Waypoint Encounter Class for Rob's Moving Map package Spherical Geometry version * @author Robert J Morton * @version 06 January 1998 */ /* Contains data and methods to transact a navigational encounter between an aircraft and a waypoint. Each encounter is a trans- action with a transient lifespan which lasts from when the aircraft enters the waypoint's range of influence until it leaves it. */ class wpenc implements navconst { private static wpenc we; // reference to current waypoint encounter object private int hS = 100; // Stealth height above terrain (in metres). private aircraft ac; // reference to current aircraft object private waypnt wp, // reference to current waypoint pw, // reference to previous waypoint nw; // reference to next waypoint private double InRad, // inbound radial from previous waypoint OutRad, // outbound radial to next waypoint rBrg, // bearing of waypoint from aircraft tBrg, // bearing of aircraft from waypoint ReqHdg, // heading the aircraft must fly pDst, // previously computed distance to waypoint Dst, // currently computed distance to waypoint tcr, // radius of aircraft's minimum desired turning circle tcd, // diameter of turning circle - double the above TCR, // square of aircraft's turning circle radius RunDst, // distance to run past this waypoint before acquiring next ThDst, // threshold approach distance to decelerate to SPEED DL, // half-width of approach corridor CM, // distance at which corridor rules end on approach RS = 50 / KphRps, // 50 kph in radians per second (min speed on landing) // VARIABLES FOR THE STEALTH HEIGHT COMMAND CHANNEL hDpw, // half-distance between current & previous waypoint hDnw, // half-distance between current and next wayppoint HDp, // half-height-difference between current & previous waypoints HDn, // half-height-difference between current & next waypoints hp, // height of previous waypoint hc, // height of current waypoint hn, // height of next waypoint CmdHgt; // height at which the aircraft is commanded to fly private boolean approach, // true if aircraft not yet passed over waypoint To, // the 'to/from' flag for the waypoint encounter Landing, // true when aircraft is in final landing phase trace = false, // when true, sends a height trace to std out FTT = true; // First-Time-Through flag for tracer // SET UP FOR APPROACH WITH NEW OUTBOUND RADIAL wpenc(waypnt w, aircraft ac, boolean Reset, boolean trace) { we = this; // reference current waypoint encounter object this.ac = ac; // reference to aircraft object this.trace = trace; FTT = true; wp = w; // get the reference of the current waypoint waypnt.setCurrent(wp); // set the reference of the current waypoint InRad = wp.getInRad(); // inbound radial from previous waypoint OutRad = wp.getOutRad(); // outbound radial to next waypoint (radians) tcr = ac.getTCR(); // get aircraft's turning circle radius tcd = tcr + tcr; // turning circle diameter ThDst = tcd + tcd; // threshold approach distance to decelerate to SPEED TCR = tcr * tcr; // square of turning radius wp.DandB(); // compute range and bearings to this waypoint Dst = wp.getDst(); // find initial distance & bearings for new waypoint RunDst = wp.getDST() / 2; // distance to run past waypoint // before capturing next CM = ac.getCM(); // corridor rules cease when aircraft gets this close pDst = Dst + .1; // initialise previous distance value approach = true; // A waypoint encounter always starts off with To = true; // the aircraft leaving the starting point DL = wp.getDL(); // half-width of approach corridor for current waypoint // INITIALIZE THE STEALTH HEIGHT COMMAND CHANNEL FOR THIS ENCOUNTER /* For HEIGHT update, I need to establish half-distances between previous & current waypoints 'hDpw' and current & next waypoint 'hDnw'. Also the half-height-difference between the current & previous waypoints 'HDp' and the current & next waypoints 'HDn'. I also need the BASE height for the calculation, which is the absolute height of the lower of the two waypoints involved in this encounter. This I must do by comparing the heights of the current waypoint with that of the next or previous way- point as appropriate. */ Landing = false; // we don't start in the landing phase! hDpw = Math.abs(wp.getPWD()/2); // half-dist from this to prev waypoint hDnw = Math.abs(wp.getDST()/2); // half-dist from this to next waypoint pw = wp.getPrev(); // reference to previous waypoint object hp = pw.getHgt(); // height of previous waypoint hc = wp.getHgt(); // height of current waypoint HDp = (double)((hc - hp)/2); // half ht diff between this & prev wpt CmdHgt = hc; // initialise command height if(!wp.getDest()) { // final waypoint does not have a next nw = wp.getNext(); // reference to next waypoint object hn = nw.getHgt(); // height of next waypoint HDn = (double)((hc - hn)/2); // half ht dif current and next wp } /* If the current waypoint is the first in the route, set the initial aircraft height to waypoint height and the command height to waypoint height + stealth height. Else, if the aircraft has just been reset to any other waypoint in the route, set both aircraft height and the com- mand height to waypoint height + stealth height. If the aircraft is en- route past any but the first waypoint, leave the heights as they are. */ if(wp.getFrst()) { ac.setHgt(hc); ac.setCmdHgt(hc + hS); } else if(Reset) { ac.setHgt(hc + hS); ac.setCmdHgt(hc + hS); } } /* ADVANCE AIRCRAFT AND UPDATE ALL ENCOUNTER VARIABLES If still approaching or not yet gone RunDst past waypoint, advance the aircraft along its track, then make last time's distance this time's previous distance, compute distance & bearings between aircraft way- point, update the command height and return that the aircraft is still encountering the waypoint. */ boolean enRoute() { if(approach || Dst < RunDst) { ac.advance(getReqHdg(), getReqSpd()); pDst = Dst; wp.DandB(); Dst = wp.getDst(); ac.setCmdHgt((int)(getCmdHgt())); /* if(trace) { if(FTT) { System.out.println("\n CmdHgt ToFlag Height"); FTT = false; } System.out.printf("%9.2f %6b", CmdHgt, To); } */ return true; } return false; } /* UPDATE THE STEALTH HEIGHT COMMAND For HEIGHT update, I need to do the sigmoid function computation. First I need the positive half-distance 'D' to the previous or next waypoint and the positive half-height difference 'H' between the current and the next or previous waypoint. If 'H' be negative, this is what I have called the 'Low' cases [See diagram in HTML text]. */ private double getCmdHgt() { double D, H, x, B = hc; // set Base height = current waypoint height if(Landing) { if(RunDst == 0) return CmdHgt; // to avoid division by zero D = RunDst; x = 1 - 2 * Dst/RunDst; H = hS/2; B = hc; } else if(To) { if(hDpw == 0) return CmdHgt; // to avoid division by zero D = hDpw; x = 1 - Dst/D; H = HDp; if(H >= 0) B = hp; } else { if(hDnw == 0) return CmdHgt; // to avoid division by zero D = hDnw; x = Dst/D - 1; H = HDn; if(H >= 0) B = hn; } /* NOTE: 'Dst' is always a positive radial distance, as is hDpw & hDnw, consequently, 'D' is always a positive amount. In the TO situation, 'x' is positive, while in the FROM situation it is negative. It is unaffected by whether the situation be HIGH or LOW. The situation is somewhat different for the landing phase. [See diagram in HTML text]. I use the bipolar sigmoid formula to compute 'y'. I determined the optimum value of 'k' to be 4.5 by experiment using sigexp.java. The formula gives a value for 'y' with the same sign as that of 'x'. The final multiplier 1.022554 was found impirically to give a better shape to the sigmoid function for stealth height profiling. NOTE: initial and final values from this formula are always NaNs. */ double y = (2 / (1 + Math.exp(-4.5 * x)) - 1) * 1.022554; /* Except when landing, the value of 'y', in the quadrants FROM & HIGH and TO & LOW, must be made negative. */ if(!Landing && (To && H < 0 || !To && H >= 0)) y = -y; /* I now have the correct sign for 'y' in every case, so to give 'Y' the correct sign, I must multiply by the positive value of 'H'. */ H = Math.abs(H); /* The value of 'Y' is referenced to the height mid-way between that of the current waypoint and the other waypoint [next or previous as appropriate] this height is the absolute value of 'H', which I already have, plus the height 'B' of the lower of the two waypoints. Finally, the stealth height 'hS' at which the air vehicle cruises above its terrain profile of its track must be added. Thus the new aircraft height that must be stored in the aircraft class is: */ CmdHgt = H * (1 + y) + B; if(!Landing) CmdHgt += hS; /* NOTE: CmdHgt is not the actual height of the aircraft. It is the in- tended 'height profile' of the aircraft's flight track. Please see aircraft.java for how 'CmdHgt' is used as a 'dynamic attractor' for the aircraft's actual height. */ return CmdHgt; } // COMPUTE THE HEADING ON WHICH THE AIRCRAFT MUST FLY private double getReqHdg() { tBrg = wp.gettBrg(); // bearing of the aircraft viewed from the waypoint rBrg = wp.getrBrg(); // bearing of the waypoint viewed from the aircraft if(Dst < pDst) // if aircraft is approaching waypoint, To = true; // set the 'to' flag true else // otherwise To = false; // set the 'to' flag false (= 'from') /* If aircraft is now receding but is still closer than its turning radius, it is deemed to have passed the waypoint. */ if(approach && !To && Dst < tcr) approach = false; /* If close enough for Euclidean approach & turn/retreat geometry, get the short-range radio heading; else (if more than 150 km from either waypoint) get trans-oceanic GPS or inertial heading. */ if(Dst < wp.getRange()) ReqHdg = getRadioHeading(); else ReqHdg = getGPSheading(); // return the rationalised (0-2π range) Required Heading return ReqHdg = ac.RatAng(ReqHdg); } // DETERMINE THE REQUIRED SPEED OF THE AIRCRAFT private double getReqSpd() { double x = Dst, // aircraft's distance from waypoint rs; // required speed /* If aircraft has passed over the destination waypoint, then decelerate, imposing a minimum speed restriction and return the required speed (to ac.advance()). */ if(wp.getDest() && !approach) { x = -1.75 * x; Landing = true; } if((rs = ac.getSPEED() * (1 + x / ThDst)) < RS) rs = RS; return rs; } // COMPUTE STEERING OFF-SET FROM WAYPOINT TO GUIDE-CIRCLE TANGENT double getStrOff(double a) { a = ac.BipAng(a - OutRad + π); // deviation from the inbound radial boolean right = true; // indicates whether radial deviation // is right or left /* If it is to the left of the inbound radial from the waypoint's point of view, make it positive and use the flag to indicate its sense. */ if(a < 0) { a = -a; right = false; } /* Now is a good time to refer to the Waypoint Encounter 'Pre- Capture' diagram in AirNav6.htm. 'e' is the square of distance to where tangent touches circle and 's' is the steering off-set: angle between bearing to waypoint and tangent. */ double b = tcr * Math.cos(a), c = Dst - tcr * Math.sin(a), e = b * b + c * c - TCR, s = 0; /* If aircraft has not yet reached the turning circle, compute steering offset from waypoint bearing required to keep the air- craft on a tangent to the turning circle. Set steering offset negative if aircraft to the right of OutRad + π. */ if(e > 0 && c != 0) { s = Math.atan2(b, c) - Math.atan2(tcr, Math.sqrt(e)); if(right) s = -s; } return s; // return the required steering offset } /* KEEP AIRCRAFT WITHIN WIND DRIFT CORRIDOR Aircraft's approach distance from waypoint, aircraft's deviation from inbound radial InRad, half-width of the approach confinement corridor. */ private double getWndOff(double a) { double q = Dst * Math.sin(ac.BipAng(a - InRad)) + DL, s = 0; // default value of corridor re-entry steering off-set /* If outside the approach confinement corridor, get the along-track side of steering triangle */ if(Math.abs(q) > Math.abs(DL)) { double e = ac.getAPD(); /* Provided it is non-zero (it's from another class!), impose an excursion limit on tangent of the steering off-set (or cor- ridor re-entry) angle and then compute the said angle. */ if(e != 0) { if((q /= e) > 10) q = +10; else if(q < -10) q = -10; s = Math.atan(q); } } return s; // return it as a steering offset from rBrg } /* The following method computes the required heading from bearing informa- tion which is relative to the waypoint's North, not the aircraft's north. However, the aircraft can only set its heading relative to its own view of which direction North is in. The returned ReqHdg will therefore be accur- ate only up to about 150km. This is of course different if the waypoint it- self is supplying steering information as in the case of an ILS. All bear- ings are relative to the waypoint. */ private double getRadioHeading() { if(approach) { // If aircraft is currently in the approach phase... double s = 0; // set default for steering off-set angle /* If still within the corrodor approach run, keep aircraft within corridor boundaries. */ if(Dst > CM) s = getWndOff(tBrg); /* If there's no steering command from corridor confinement, invoke normal approach steering. */ if(s == 0) s = getStrOff(tBrg); /* Do not disturb last time's ReqHdg value if 's' is zero set bearing of waypoint from aircraft + steering correction. */ if(s != 0) ReqHdg = rBrg + s; } else { // Else, if receding, track the waypoint's outbound radial double d = Dst * Math.sin(ac.BipAng(OutRad - tBrg)), e = ac.getAPD(); // base of steering triangle /* Better check first that 'e' is non-zero because it is from another class! Impose an excusion limit on the tangent of the steering off-set angle. Then off-set the required heading by the amount required to bring aircraft back on track along the outbound radial to the next waypoint. */ if(e != 0) { if((d /= e) > 10) d = +10; else if(d < -10) d = -10; ReqHdg = OutRad + Math.atan(d); } } return ReqHdg; // return required heading } /* The following method returns a Required Heading value which is relative to the aircraft's own perception of where North is. However, its ability to correct for wind drift gets worse the greater the distance between way- points. There is a better (but more complicated) treatment which makes the corrective angle proportional to the actual distance off course. All bear- ings are relative to the aircraft. */ private double getGPSheading() { /* If approaching current waypoint, compute bearing of previous waypoint; else aircraft is receding from current waypoint, so compute bearing of next waypoint and steer twice the reverse of the difference between the bearing of the next waypoint and the extended bearing line from the previous waypoint. */ if(To) { pw.DandB(); ReqHdg = 2 * rBrg - pw.getrBrg() - π; } else { nw.DandB(); ReqHdg = 2 * nw.getrBrg() - rBrg - π; } return ReqHdg; } boolean getToFlag() {return To;} // return the approach status // return reference to current waypoint encounter object static wpenc getCurrent() {return we;} }