From afb654a068ffed6e8157530c0a4d8fcc39e7d0f9 Mon Sep 17 00:00:00 2001 From: daveluff Date: Fri, 23 Jan 2004 17:16:15 +0000 Subject: [PATCH] Initial revision of class for AI VFR GA traffic --- src/ATC/AIGAVFRTraffic.cxx | 450 +++++++++++++++++++++++++++++++++++++ src/ATC/AIGAVFRTraffic.hxx | 132 +++++++++++ 2 files changed, 582 insertions(+) create mode 100644 src/ATC/AIGAVFRTraffic.cxx create mode 100644 src/ATC/AIGAVFRTraffic.hxx diff --git a/src/ATC/AIGAVFRTraffic.cxx b/src/ATC/AIGAVFRTraffic.cxx new file mode 100644 index 000000000..2078ef6dc --- /dev/null +++ b/src/ATC/AIGAVFRTraffic.cxx @@ -0,0 +1,450 @@ +// FGAILocalTraffic - AIEntity derived class with enough logic to +// fly and interact with the traffic pattern. +// +// Written by David Luff, started March 2002. +// +// Copyright (C) 2002 David C. Luff - david.luff@nottingham.ac.uk +// +// 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. +// +// 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. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + +#ifdef HAVE_CONFIG_H +# include +#endif + +//#include + +#include +#include
+//#include +//#include +#include +//#include +//#include +#include +#include + +SG_USING_STD(string); + +#include "ATCmgr.hxx" +#include "AILocalTraffic.hxx" +#include "AIGAVFRTraffic.hxx" +#include "ATCutils.hxx" + +FGAIGAVFRTraffic::FGAIGAVFRTraffic() { + ATC = globals->get_ATC_mgr(); + _towerContactedIncoming = false; + _clearedStraightIn = false; + _clearedDownwindEntry = false; + _incoming = false; + _straightIn = false; + _downwindEntry = false; + _climbout = false; + _local = false; + _established = false; + _e45 = false; + _entering = false; + _turning = false; + _cruise_climb_ias = 90.0; + _cruise_ias = 110.0; + patternDirection = -1.0; + + // TESTING - REMOVE OR COMMENT OUT BEFORE COMMIT!!! + //_towerContactPrinted = false; +} + +FGAIGAVFRTraffic::~FGAIGAVFRTraffic() { +} + +// We should never need to Init FGAIGAVFRTraffic in the pattern since that implies arrivel +// and we can just use an FGAILocalTraffic instance for that instead. + +// Init en-route to destID at point pt. +// TODO - no idea what to do if pt is above planes ceiling due mountains!! +bool FGAIGAVFRTraffic::Init(Point3D pt, string destID, const string& callsign) { + FGAILocalTraffic::Init(callsign, destID, EN_ROUTE); + // TODO FIXME - to get up and running we're going to ignore elev and get FGAIMgr to + // pass in known good values for the test location. Need to fix this!!! (or at least canonically decide who has responsibility for setting elev). + _enroute = true; + _destID = destID; + _pos = pt; + _destPos = dclGetAirportPos(destID); // TODO - check if we are within the tower catchment area already. + _cruise_alt = (_destPos.elev() + 2500.0) * SG_FEET_TO_METER; // TODO look at terrain elevation as well + _pos.setelev(_cruise_alt); + // initially set waypoint as airport location + _wp = _destPos; + _hdg = GetHeadingFromTo(_pos, _wp); + _roll = 0.0; + _pitch = 0.0; + slope = 0.0; + // TODO - set climbout if altitude is below normal cruising altitude? + //Transform(); + // Assume it's OK to set the plane visible + _aip.setVisible(true); + //cout << "Setting visible true\n"; + Transform(); + return(true); +} + +// Init at srcID to fly to destID +bool FGAIGAVFRTraffic::Init(string srcID, string destID, const string& callsign, OperatingState state) { + _enroute = false; + FGAILocalTraffic::Init(callsign, srcID, PARKED); + return(true); +} + +void FGAIGAVFRTraffic::Update(double dt) { + if(_enroute) { + //cout << "_enroute\n"; + //cout << "e" << flush; + FlyPlane(dt); + //cout << "f" << flush; + Transform(); + //cout << "g" << flush; + FGAIPlane::Update(dt); + //cout << "h" << flush; + responseCounter += dt; + + // we shouldn't really need this since there's a LOD of 10K on the whole plane anyway I think. + // There are two _aip.setVisible statements set when _local = true that can be removed if the below is removed. + if(dclGetHorizontalSeparation(_pos, Point3D(fgGetDouble("/position/longitude-deg"), fgGetDouble("/position/latitude-deg"), 0.0)) > 8000) _aip.setVisible(false); + else _aip.setVisible(true); + + } else if(_local) { + //cout << "L"; + //cout << "_local\n"; + FGAILocalTraffic::Update(dt); + } +} + +void FGAIGAVFRTraffic::FlyPlane(double dt) { + if(_climbout) { + // Check whether to level off + if(_pos.elev() >= _cruise_alt) { + slope = 0.0; + _pitch = 0.0; + IAS = _cruise_ias; // FIXME - use smooth transistion to new speed and attitude. + _climbout = false; + } else { + slope = 4.0; + _pitch = 5.0; + IAS = _cruise_climb_ias; + } + } else { + // TESTING + /* + if(dclGetHorizontalSeparation(_destPos, _pos) / 1600.0 < 8.1) { + if(!_towerContactPrinted) { + if(airportID == "KSQL") { + cout << "****************************************************************\n"; + cout << "++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n"; + cout << "****************************************************************\n"; + } + _towerContactPrinted = true; + } + } + */ + + // if distance to destination is less than 6 - 9 miles contact tower + // and prepare to become _incoming after response. + // Possibly check whether to start descent before this? + //cout << "." << flush; + //cout << "sep = " << dclGetHorizontalSeparation(_destPos, _pos) / 1600.0 << '\n'; + if(dclGetHorizontalSeparation(_destPos, _pos) / 1600.0 < 8.0) { + //cout << "-" << flush; + if(!_towerContactedIncoming) { + //cout << "_" << flush; + GetAirportDetails(airportID); + //cout << "L" << flush; + // TODO FIXME TODO - need to check that tower is valid before this else if problem -> BOOM! + freq = (double)tower->get_freq() / 100.0; + tuned_station = tower; + //cout << "freq = " << freq << endl; + GetRwyDetails(airportID); + //"@AP Tower @CS @MI miles @CD of the airport for full stop with the ATIS" + // At the bare minimum we ought to make sure it goes the right way at dual parallel rwy airports! + if(rwy.rwyID.size() == 3) { + patternDirection = (rwy.rwyID.substr(2,1) == "R" ? 1 : -1); + } + pending_transmission = tower->get_name(); + pending_transmission += " Tower "; + pending_transmission += plane.callsign; + //char buf[10]; + int dist_miles = (int)dclGetHorizontalSeparation(_pos, _destPos) / 1600; + //sprintf(buf, " %i ", dist_miles); + pending_transmission += " "; + pending_transmission += ConvertNumToSpokenDigits(dist_miles); + if(dist_miles > 1) pending_transmission += " miles "; + else pending_transmission += " mile "; + pending_transmission += GetCompassDirection(GetHeadingFromTo(_destPos, _pos)); + pending_transmission += " of the airport for full stop with the ATIS"; + //cout << pending_transmission << endl; + Transmit(14); // 14 is the callback code, NOT the timeout! + responseCounter = 0; + _towerContactedIncoming = true; + } else { + //cout << "?" << flush; + if(_clearedStraightIn && responseCounter > 5.5) { + //cout << "5 " << flush; + _clearedStraightIn = false; + _straightIn = true; + _incoming = true; + _wp = GetPatternApproachPos(); + _hdg = GetHeadingFromTo(_pos, _wp); // TODO - turn properly! + slope = atan((_wp.elev() - _pos.elev()) / dclGetHorizontalSeparation(_wp, _pos)) * DCL_RADIANS_TO_DEGREES; + double thesh_offset = 0.0; + Point3D opos = ortho.ConvertToLocal(_pos); + double angToApt = atan((_pos.elev() - dclGetAirportElev(airportID)) / (opos.y() - thesh_offset)) * DCL_RADIANS_TO_DEGREES; + //cout << "angToApt = " << angToApt << ' '; + slope = (angToApt > -5.0 ? 0.0 : angToApt); + //cout << "slope = " << slope << '\n'; + pending_transmission = "Straight-in "; + pending_transmission += ConvertRwyNumToSpokenString(rwy.rwyID); + pending_transmission += " "; + pending_transmission += plane.callsign; + //cout << pending_transmission << '\n'; + ConditionalTransmit(4); + } else if(_clearedDownwindEntry && responseCounter > 5.5) { + //cout << "6" << flush; + _clearedDownwindEntry = false; + _downwindEntry = true; + _incoming = true; + _wp = GetPatternApproachPos(); + _hdg = GetHeadingFromTo(_pos, _wp); // TODO - turn properly! + slope = atan((_wp.elev() - _pos.elev()) / dclGetHorizontalSeparation(_wp, _pos)) * DCL_RADIANS_TO_DEGREES; + //cout << "slope = " << slope << '\n'; + pending_transmission = "Report "; + pending_transmission += (patternDirection == 1 ? "right downwind " : "left downwind "); + pending_transmission += ConvertRwyNumToSpokenString(rwy.rwyID); + pending_transmission += " "; + pending_transmission += plane.callsign; + //cout << pending_transmission << '\n'; + ConditionalTransmit(4); + } + } + if(_pos.elev() < (dclGetAirportElev(airportID) + (1000.0 * SG_FEET_TO_METER))) slope = 0.0; + } + } + if(_incoming) { + //cout << "i" << '\n'; + Point3D orthopos = ortho.ConvertToLocal(_pos); + // TODO - Check whether to start descent + // become _local after the 3 mile report. + if(_pos.elev() < (dclGetAirportElev(airportID) + (1000.0 * SG_FEET_TO_METER))) slope = 0.0; + // TODO - work out why I needed to add the above line to stop the plane going underground!!! + // (Although it's worth leaving it in as a robustness check anyway). + if(_straightIn) { + //cout << "A " << flush; + if(fabs(orthopos.x()) < 10.0 && !_established) { + _hdg = rwy.hdg; // MEGA MEGA HACK - FIXME!!!!!!! + _established = true; + //cout << "Established at " << orthopos << '\n'; + } + double thesh_offset = 30.0; + //cout << "orthopos.y = " << orthopos.y() << " alt = " << _pos.elev() - dclGetAirportElev(airportID) << '\n'; + if(_established && (orthopos.y() > -5400.0)) { + slope = atan((_pos.elev() - dclGetAirportElev(airportID)) / (orthopos.y() - thesh_offset)) * DCL_RADIANS_TO_DEGREES; + //cout << "slope0 = " << slope << '\n'; + } + //cout << "slope1 = " << slope << '\n'; + if(slope > -5.5) slope = 0.0; // ie we're too low. + //cout << "slope2 = " << slope << '\n'; + slope += 0.001; // To avoid yo-yoing with the above. + //if(_established && (orthopos.y() > -5400.0)) slope = -5.5; + if(_established && (orthopos.y() > -4800.0)) { + pending_transmission = "3 mile final Runway "; + pending_transmission += ConvertRwyNumToSpokenString(rwy.rwyID); + pending_transmission += " "; + pending_transmission += plane.callsign; + //cout << pending_transmission << '\n'; + ConditionalTransmit(35); + _local = true; + _aip.setVisible(true); // HACK + _enroute = false; + StraightInEntry(true); + } + } else if(_downwindEntry) { + //cout << "B" << flush; + if(_entering) { + //cout << "C" << flush; + if(_turning) { + double tgt_hdg = rwy.hdg + 180.0; + while((tgt_hdg - _hdg) > 180.0) _hdg += 360.0; + while((_hdg - tgt_hdg) > 180.0) _hdg -= 360.0; + double turn_time = 60.0; + _hdg += (360.0 / turn_time) * dt * (tgt_hdg > _hdg ? 1.0 : -1.0); + Bank(25.0 * (tgt_hdg > _hdg ? 1.0 : -1.0)); + if(fabs(_hdg - tgt_hdg) < 2.0) { + //cout << "Going Local...\n"; + _hdg = rwy.hdg + 180.0; // TODO - FIX THIS UGLY HACK!!!!!!! + leg = DOWNWIND; + _local = true; + _aip.setVisible(true); // HACK + _enroute = false; + _entering = false; + _turning = false; + DownwindEntry(); + } + } + if(fabs(orthopos.x() - (patternDirection == 1 ? 1000 : -1000)) < (_e45 ? 175 : 550)) { // Caution - hardwired turn clearances. + //cout << "_turning...\n"; + _turning = true; + } // TODO - need to check for other traffic in the pattern and enter much more integilently than that!!! + } else { + //cout << "D" << flush; + //cout << '\n' << dclGetHorizontalSeparation(_wp, _pos) << '\n'; + //cout << ortho.ConvertToLocal(_pos); + //cout << ortho.ConvertToLocal(_wp); + if(dclGetHorizontalSeparation(_wp, _pos) < 100.0) { + pending_transmission = "2 miles out for "; + pending_transmission += (patternDirection == 1 ? "right " : "left "); + pending_transmission += "downwind Runway "; + pending_transmission += ConvertRwyNumToSpokenString(rwy.rwyID); + pending_transmission += " "; + pending_transmission += plane.callsign; + //cout << pending_transmission << '\n'; + // TODO - are we at pattern altitude?? + slope = 0.0; + ConditionalTransmit(30); + if(_e45) { + _hdg = (patternDirection == 1 ? rwy.hdg - 135.0 : rwy.hdg + 135.0); + } else { + _hdg = (patternDirection == 1 ? rwy.hdg + 90.0 : rwy.hdg - 90.0); + } + if(_hdg < 0.0) _hdg += 360.0; + _entering = true; + } + } + } + } else { + // !_incoming + slope = 0.0; + } + // FIXME - lots of hackery in the next six lines!!!! + double track = _hdg; + double crab = 0.0; + _hdg = track + crab; + double vel = _cruise_ias; + double dist = vel * 0.514444 * dt; + _pos = dclUpdatePosition(_pos, track, slope, dist); +} + +void FGAIGAVFRTraffic::RegisterTransmission(int code) { + switch(code) { + case 1: // taxi request cleared + FGAILocalTraffic::RegisterTransmission(code); + break; + case 2: // contact tower + FGAILocalTraffic::RegisterTransmission(code); + break; + case 3: // Cleared to line up + FGAILocalTraffic::RegisterTransmission(code); + break; + case 4: // cleared to take-off + FGAILocalTraffic::RegisterTransmission(code); + break; + case 5: // contact ground + FGAILocalTraffic::RegisterTransmission(code); + break; + case 6: // taxi to the GA parking + FGAILocalTraffic::RegisterTransmission(code); + break; + case 7: // Cleared to land + FGAILocalTraffic::RegisterTransmission(code); + break; + case 13: // Go around! + FGAILocalTraffic::RegisterTransmission(code); + break; + case 14: // VFR approach for straight-in + responseCounter = 0; + _clearedStraightIn = true; + break; + case 15: // VFR approach for downwind entry + responseCounter = 0; + _clearedDownwindEntry = true; + break; + default: + break; + } +} + +// Callback handler +// TODO - Really should enumerate these coded values. +void FGAIGAVFRTraffic::ProcessCallback(int code) { + // 1 - Request Departure from ground + // 2 - Report at hold short + // 10 - report crosswind + // 11 - report downwind + // 12 - report base + // 13 - report final + // 14 - Contact Tower for VFR arrival + if(code < 14) { + FGAILocalTraffic::ProcessCallback(code); + } else if(code == 14) { + tower->VFRArrivalContact(plane, this, FULL_STOP); + } +} + +// Return an appropriate altitude to fly at based on the desired altitude and direction +// whilst respecting the quadrangle rule. +int FGAIGAVFRTraffic::GetQuadrangleAltitude(int dir, int des_alt) { + return(8888); + // TODO - implement me! +} + +// Calculates the position needed to set up for either pattern entry or straight in approach. +// Currently returns one of three positions dependent on initial position wrt threshold of active rwy. +// 1/ A few miles out on extended centreline for straight-in. +// 2/ At an appropriate point on circuit side of rwy for a 45deg entry to downwind. +// 3/ At and appropriate point on non-circuit side of rwy at take-off end for perpendicular entry to circuit overflying end-of-rwy. +Point3D FGAIGAVFRTraffic::GetPatternApproachPos() { + //cout << "\n\n"; + //cout << "PPPPPPPPPPPPPPPPPPPPPPPppppppppppppppp\n"; + //cout << "Calculating pattern approach pos for " << plane.callsign << '\n'; + Point3D orthopos = ortho.ConvertToLocal(_pos); + Point3D tmp; + //cout << "patternDirection = " << patternDirection << '\n'; + if(orthopos.y() >= -1000.0) { // Note that this has to be set the same as the calculation in tower.cxx - at the moment approach type is not transmitted properly between the two. + //cout << "orthopos.x = " << orthopos.x() << '\n'; + if((orthopos.x() * patternDirection) > 0.0) { // 45 deg entry + tmp.setx(2000 * patternDirection); + tmp.sety((rwy.end2ortho.y() / 2.0) + 2000); + tmp.setelev(dclGetAirportElev(airportID) + (1000 * SG_FEET_TO_METER)); + _e45 = true; + //cout << "45 deg entry... "; + } else { + tmp.setx(1000 * patternDirection * -1); + tmp.sety(rwy.end2ortho.y()); + tmp.setelev(dclGetAirportElev(airportID) + (1000 * SG_FEET_TO_METER)); + _e45 = false; + //cout << "90 deg entry... "; + } + } else { + tmp.setx(0); + tmp.sety(-5400); + tmp.setelev((5400.0 / 6.0) + dclGetAirportElev(airportID) + 10.0); + //cout << "Straight in... "; + } + //cout << "Waypoint is " << tmp << '\n'; + //cout << ortho.ConvertFromLocal(tmp) << '\n'; + //cout << '\n'; + //exit(-1); + return ortho.ConvertFromLocal(tmp); +} + +//FGAIGAVFRTraffic:: + +//FGAIGAVFRTraffic:: + +//FGAIGAVFRTraffic:: diff --git a/src/ATC/AIGAVFRTraffic.hxx b/src/ATC/AIGAVFRTraffic.hxx new file mode 100644 index 000000000..e2bbb5733 --- /dev/null +++ b/src/ATC/AIGAVFRTraffic.hxx @@ -0,0 +1,132 @@ +// FGAILocalTraffic - AIEntity derived class with enough logic to +// fly and interact with the traffic pattern. +// +// Written by David Luff, started March 2002. +// +// Copyright (C) 2002 David C. Luff - david.luff@nottingham.ac.uk +// +// 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. +// +// 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. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + +#ifndef _FG_AIGAVFRTraffic_HXX +#define _FG_AIGAVFRTraffic_HXX + +#include +#include +#include +#include
+ +#include "tower.hxx" +#include "AIPlane.hxx" +#include "ATCProjection.hxx" +#include "ground.hxx" +#include "AILocalTraffic.hxx" + +#include +SG_USING_STD(string); + +class FGAIGAVFRTraffic : public FGAILocalTraffic { + +public: + + FGAIGAVFRTraffic(); + ~FGAIGAVFRTraffic(); + + // Init en-route to destID at point pt. (lat, lon, elev) (elev in meters, lat and lon in degrees). + bool Init(Point3D pt, string destID, const string& callsign); + // Init at srcID to fly to destID + bool Init(string srcID, string destID, const string& callsign, OperatingState state = PARKED); + + // Run the internal calculations + void Update(double dt); + + // Return what type of landing we're doing on this circuit + //LandingType GetLandingOption(); + + void RegisterTransmission(int code); + + // Process callbacks sent by base class + // (These codes are not related to the codes above) + void ProcessCallback(int code); + +protected: + + // Do what is necessary to land and parkup at home airport + void ReturnToBase(double dt); + + //void GetRwyDetails(string id); + + +private: + FGATCMgr* ATC; + // This is purely for synactic convienience to avoid writing globals->get_ATC_mgr()-> all through the code! + + // High-level stuff + OperatingState operatingState; + bool touchAndGo; //True if circuits should be flown touch and go, false for full stop + + // Performance characteristics of the plane in knots and ft/min - some of this might get moved out into FGAIPlane + double best_rate_of_climb_speed; + double best_rate_of_climb; + double nominal_climb_speed; + double nominal_climb_rate; + double nominal_cruise_speed; + double nominal_circuit_speed; + double nominal_descent_rate; + double nominal_approach_speed; + double nominal_final_speed; + double stall_speed_landing_config; + + // environment - some of this might get moved into FGAIPlane + SGPropertyNode* wind_from_hdg; //degrees + SGPropertyNode* wind_speed_knots; //knots + + atc_type changeFreqType; // the service we need to change to + + void CalculateSoD(double base_leg_pos, double downwind_leg_pos, bool pattern_direction); + + // GA VFR specific + bool _towerContactedIncoming; + bool _straightIn; + bool _clearedStraightIn; + bool _downwindEntry; + bool _clearedDownwindEntry; + Point3D _wp; // Next waypoint (ie. the one we're currently heading for) + bool _enroute; + string _destID; + bool _climbout; + double _cruise_alt; + double _cruise_ias; + double _cruise_climb_ias; + Point3D _destPos; + bool _local; + bool _incoming; + bool _established; + bool _e45; + bool _entering; + bool _turning; + + //ssgBranch* _model; + + int GetQuadrangleAltitude(int dir, int des_alt); + + Point3D GetPatternApproachPos(); + + void FlyPlane(double dt); + + // HACK for testing - remove or comment out before CVS commit!!! + //bool _towerContactPrinted; +}; + +#endif // _FG_AILocalTraffic_HXX